我的AI之路(28)--基于ROSBridgeClient实现安卓App和ROS之间的通讯
有操作屏幕的机器人通常首选安卓系统作为屏幕UI应用的开发和运行环境,机器人的导航和运动等底盘控制则通常都是基于Linux+ROS实现的,跑安卓系统的板子通常俗称上位机,跑Linux+ROS的则叫下位机,为完成机器人的整体功能控制,上下位机之间是需要通讯的,另外如果有专门的特殊设备(比如执行和传输数据频繁的实时性要求高的设备,这样的设备通常都首选只支持Linux)跑起来如果比较耗资源,连接到上位机或者下位机上跑都会影响整体系统性能,那么单独为这样的设备准备一块跑Linux的ARM或者X86板子是必须的,这样的板子上的程序也往往需要由上位机的安卓程序调用和控制。
对于这样的不同板子之间不同OS和语言实现的程序之间的通讯,这时可以自己写C/C++代码实现基于Socket甚至是WebSocket方式的通讯,并使用安卓和Linux各自的交叉编译工具编译成对应OS的可执行程序或者供调用的so库,这显然是个工作量比较大的重量级实现方案,其次,也可以在安卓上使用ROSJava开发实现ROS节点来与Linux上的ROS节点进行通讯,这也是个有一定工作量的实现途径,并且ROSJava被公认的体量有点大,前面说过(参见我的AI之路(25)--ROSBridge:机器人与外部系统之间的通讯解决方案),ROSBridge是一个很好的异构环境之间的通讯实现框架,采用它可以节省工作量快速实现方案,具体的通讯方式取决于实际需要,如果需要长连接且双方之间是P2P关系不是C/S关系(都有主动向对方发送数据的需要),那么选择WebSocket通讯是非常合适的,我们又知道ROSBridge只实现了WebSocket Server端,只能作为Server运行,如果能有一种基于Java实现的、实现了WebSocket Client的框架可供使用,那么可以大大节省工作量,如果还能按照ROSBridge通讯协议规范定义rosbridge v2.0 Protocol Specification中的要求将通讯数据自动打包成相应的格式发送给RosBridge的WebSocket Server并且能自动接收和解析来自RosBridge的WebSocket Server的数据供安卓应用程序使用,那就几乎完美了,几乎只需要很少的编程就可以快速实现安卓和Linux+ROS之间的通讯了!
ROSBridgeClient就是这么一个框架,这个项目没有jar形式的发布包,可以使用
git clone https://github.com/djilk/ROSBridgeClient.git
命令把源码下下来,直接集成到你的安卓项目中即可,这些源码里使用到了Java WebSocket,这个可以配置maven central repository下载或者下载发布版的jar包,如果选择下载jar包,最好选择Java-WebSocket-1.4.0-with-dependencies.jar而不是Java-WebSocket-1.4.0.jar,为什么呢?因为Java WebSokcket里调用到了SLF4J来实现日志功能,相关的支持包就一起打包到了Java-WebSocket-1.4.0-with-dependencies.jar里了,这样用起来太方便了,直接放到你的安卓项目的libs下面再在你的build.gradle文件里加句:
implementation files('libs/Java-WebSocket-1.4.0-with-dependencies.jar')
然后sync就可以用了。 另外在构造和解析rosbridge v2.0 Protocol Specification规定的json格式数据时使用到了json-simple,可以配置maven库下载或者点击http://central.maven.org/maven2/com/googlecode/json-simple/json-simple/1.1.1/里的json-simple-1.1.1.jar下载这个jar包放到libs下面后在build.gradle里增加相关的implementation语句即可,不过我更喜欢使用功能更强大性能更好的fastjson,因此把RosBridgeClient的源码com.jilk.ros.rosbridge.implementation.JSON.java里用到json-simple调用的地方全部修改成了对fastjson的调用,另外这个文件和里面的类名也叫JSON,在使用快捷建引入到其他文件里时老不小心容易跟fastjson里的JSON混淆弄错,而且它其实只是个Util类,因此干脆把文件名和类名都改叫JSONUtil了。
不过ROSBridgeClient这个框架貌似一个没完工的半拉子项目,里面设置还有错误需要你自己修改,不然用不上全部功能,这点很奇怪,以前没见过这样的开源项目,关于这点后面说,先分析理解一下它的主要调用关系,对框架代码有了较整体的理解才能动手修改或完善它。
不用每个文件都去看一遍,从它给出的示例文件com.jilk.ros.Example入手分析调用关系可以很快弄明白:
public static void main(String[] args) {
ROSBridgeClient client = new ROSBridgeClient("ws://162.243.238.80:9090"); //UIR当然是根据你实际IP和端口设置
client.connect(); //内部创建ROSBridgeWebSocketClient对象并调用其方法connectBlocking()从而连接到RosBrige WebSocket Server
//testTopic(client);
try {
testService(client);
}
catch (RuntimeException ex) {
ex.printStackTrace();
}
finally {
client.disconnect();
}
}
public static void testService(ROSBridgeClient client) {
try {
Service<Empty, GetTime> timeService = new Service<Empty, GetTime>("/rosapi/get_time", Empty.class, GetTime.class, client);
timeService.verify();
System.out.println("Time (secs): " + timeService.callBlocking(new Empty()).time.secs); //callBlocking()是等到RosBridge返回结果数据才返回的,在结果数据返回之前,通过latch机制一直阻住
...
public class ROSBridgeClient extends ROSClient {
...
public ROSBridgeClient(String uriString) {
this.uriString = uriString;
}
public boolean connect() {
return connect(null);
}
public boolean connect(ROSClient.ConnectionStatusListener listener) {
boolean result = false;
client = ROSBridgeWebSocketClient.create(uriString);
if (client != null) {
client.setListener(listener);
try {
result = client.connectBlocking();
}
catch (InterruptedException ex) {}
}
return result;
}
public void register(Class<? extends Operation> c, String s, Class<? extends Message> m, FullMessageHandler h) {
client.register(c, s, m, h); }
...
public class Service<CallType extends Message, ResponseType extends Message> extends Message
implements FullMessageHandler {
...
public Service(String service, Class<? extends CallType> callType, Class<? extends ResponseType> responseType, ROSClient client) {
this.service = service;
this.client = client;
this.responseType = responseType;
this.callType = callType;
calls = new HashMap<String, CallRecord>();
}
...
public ResponseType callBlocking(CallType args) throws InterruptedException {
return take(call(args));
}
public ResponseType take(String id) throws InterruptedException {
CallRecord call = calls.get(id);
call.latch.await(); //阻住,直到结果返回后 onMessage()调用call.latch.countDown()
calls.remove(id);
return call.result;
}
public String call(CallType args) {
return callImpl(args, null);
}
private String callImpl(CallType args, MessageHandler<ResponseType> responseHandler) {
client.register(ServiceResponse.class, service, responseType, this); //这里把Service对象自己注册成响应数据的handler
CallService messageCallService = new CallService(service, args); //CallService 派生自Operation
String id = messageCallService.id;
CallRecord callRecord = new CallRecord(responseHandler);
calls.put(id, callRecord); //记录operation id和callRecord的对应关系,供service call结果返回后使用
client.send(messageCallService); //调用ROSBridgeClient的public void send(Operation operation) { client.send(operation); } 从而调用ROSBridgeWebSocketClient的send(Operation operation)把operation(即CallService )数据转换成符合rosbridge v2.0 Protocol Specification要求的json格式数据然后发给RosBridge WebSocket Server,细节可以进一步看代码
return id;
}
public void onMessage(String id, Message response) { //由ROSBridgeWebSocketClient.onMessage() 调用
//System.out.print("Service.onMessage: "); //response.print(); CallRecord call = calls.get(id); //根据operation id找到service call对应的CallRecord if(call == null) { System.out.print("No caller service response"); return; } if (call.handler != null) { calls.remove(id); call.handler.onMessage((ResponseType) response); } else { call.result = (ResponseType) response; call.latch.countDown(); //service调用的对应结果已返回,释放latch } }
public class ROSBridgeWebSocketClient extends WebSocketClient {
...
public void register(Class<? extends Operation> c,
String s,
Class<? extends Message> m,
FullMessageHandler h) {
Message.register(m, classes.get(Message.class));
classes.register(c, s, m);
if (h != null)
handlers.register(c, s, h); //把service作为(c,s)对应的handler注册
}
...
public void onMessage(String message) {
if (debug) System.out.println("<ROS " + message);
//System.out.println("ROSBridgeWebSocketClient.onMessage (message): " + message);
Operation operation = Operation.toOperation(message, classes); //解析从RosBridge发回的结果数据填充到ServiceResponse对象中,ServiceResponse亦派生自Operation
//System.out.println("ROSBridgeWebSocketClient.onMessage (operation): ");
//operation.print();
FullMessageHandler handler = null;
Message msg = null;
if (operation instanceof Publish) {
Publish p = (Publish) operation;
handler = handlers.lookup(Publish.class, p.topic);
msg = p.msg;
} else if (operation instanceof ServiceResponse) {
ServiceResponse r = ((ServiceResponse) operation);
handler = handlers.lookup(ServiceResponse.class, r.service); //以(c,s)为条件从register中查找出对应的handler,即Service对象
msg = r.values;
}
if (handler != null/*&& message.contains("\"id\":")*/)
handler.onMessage(operation.id, msg); //调用Service.onMessage()交由Service处理
else {
...
示例代码中还有调用testTopic()展示怎么使用advertise/publish/subscribe/unsubscribe/unadvertise,主要通讯调用过程比较简单不难看懂,看看Topic的各种命令消息如何往RosBridge WebSocket Server发送和如何接收和处理来自RosBridge WebSocket Server的(subscribe)数据的:
public class Topic<T extends Message> extends LinkedBlockingQueue<T> implements FullMessageHandler {
...
public void advertise() {
send(new Advertise(topic, messageType));
}
public void publish(T message) {
send(new Publish(topic, message));
}
public void subscribe(MessageHandler<T> handler) {
startRunner(handler); //handler以独立线程形式运行用来读取queue中的来自RosBridge WebSocket Server的数据并做处理
subscribe();
}
public void subscribe() {
client.register(Publish.class, topic, type, this); //这里把Topic对象自己注册成响应数据的handler
send(new Subscribe(topic, messageType));
}
private void send(Operation operation) {
client.send(operation); //由ROSBridgeWebSocketClient.send(Operation operation)把数据按照RosBridge Protocol v2.0要求打包成json数据然后发送给ROSBridge WebSocket Server
}
public void onMessage(String id, Message message) { //由ROSBridgeWebSocketClient.onMessage() 调用
add((T) message); //简单地把来自ROSBridge WebSocket Server的数据加入到queue中,订阅消息时调用的subscribe(MessageHandler<T> handler)则由MessageHandler.onMessage()来调用take()取出数据并做处理,或者订阅消息时调用的subscribe()则由使用Topic对象的程序调用Topic.take()来取出数据并做处理
}
Topic使用的示例代码参见Example.java中的testTopic():
public static void testTopic(ROSBridgeClient client) {
Topic<Clock> clockTopic = new Topic<Clock>("/clock", Clock.class, client);
clockTopic.subscribe(); //使用没有handler的subscribe()
try {
Thread.sleep(20000);} catch(InterruptedException ex) {}
Clock cl = null;
try {
cl = clockTopic.take(); //上面调用的没有handler的subcribe(),因此需要自行调用Topic实例对象的take()来取出预订的来自RosBridge WebSocket Server的消息
}
catch (InterruptedException ex) {}
cl.print();
cl.clock.nsecs++;
clockTopic.unsubscribe();
clockTopic.advertise();
clockTopic.publish(cl); //简单把修改了nsecs值的clockTopic数据发布回"/clock"主题
clockTopic.unadvertise();
}
上面的Service调用的代码是调用ROS的rosapi提供的service,如果我们要调用自己开发的ROS Service(下面以FaceDetectService为例),可以类似这么写:
Service<FaceDetectServiceRequest, FaceDetectServiceResponse> faceDetectionService =
new Service<FaceDetectServiceRequest, FaceDetectServiceResponse>("FaceDetectService", FaceDetectServiceRequest.class, FaceDetectServiceResponse.class, client);
String fdResult = faceDetectionService.callBlocking(null).respStr; //阻塞式调用,直到返回结果数据respStr。请求数据无(null)
Log.v(TAG,"fdResult == " + fdResult);
jsonObject = (JSONObject) com.alibaba.fastjson.JSON.parse(fdResult); //把结果数据解析到json对象中
bFound = jsonObject.getBoolean("found"); //开始获取你需要的数据
... //后面具体有关具体数据代码涉及商业秘密,不能列出来 :)
其中FaceDetectServiceRequest, FaceDetectServiceResponse是自定义的,都必须派生自Message:
@MessageType(string = "std_msgs/String")
public class FaceDetectServiceRequest extends Message {
public String reqStr;
public FaceDetectServiceRequest(String reqStr){
this.reqStr = reqStr;
}
}
@MessageType(string = "std_msgs/String")
public class FaceDetectServiceResponse extends Message {
public String respStr;
}
ROS Service开发和运行环境方面,需在Ubuntu Linux上安装ROS及ROSBridge suite当然是首先要做的(怎么安装ROS参见我的AI之路(24)--ROS环境安装,怎么安装RosBridge suite参见我的AI之路(25)--ROSBridge:机器人与外部系统之间的通讯解决方案),然后,写FaceDetectService的实现代码并在Linux上用catkin_make编译出相应的Service节点代码的可执行程序,并且执行下面的命令启动rescore和RosBridge WebSocket Server后启动service节点:
roscore
roslaunch facedetect facedetect.launch //你自己写一个facedetect.launch文件以用来启动facedetect可执行程序
roslaunch rosbridge_server rosbridge_websocket.launch //执行rosbridge_server的rosbridge_websocket.launch
然后就可以执行上面的安卓代码调用这个service了。
另外,为保证通讯的可用性和方便管理,安卓App和ROS之间的通讯在网络中断后一般应能自动恢复连接,针对这种需求,ROSBridgeWebSocketClient和它的代理类ROSBridgeClient都支持在建立连接时设置ROSClient.ConnectionStatusListener listener,我们可以在这个listener的事件处理函数里调用connect()来实现网络连接在中断后自动建立连接:
private final ROSClient.ConnectionStatusListener csl = new ROSClient.ConnectionStatusListener() {
@Override
public void onConnect() {
client.setDebug(true);
Log.d(TAG, "ROS connect");
}
@Override
public void onDisconnect(boolean normal, String reason, int code) {
Log.d(TAG, "ROS disconnect, normal:"+normal+",reason:"+reason+",code:"+code);
if(code!=1000) // 1000: CLOSE_NORMAL
client.connect(this);
}
@Override
public void onError(Exception ex) {
Log.d(TAG, "ROS error");
ex.printStackTrace();
client.connect(this);
}
};
client = new ROSBridgeClient("ws://"+ip+":"+port);
client.connect(csl);
RosBridgeClient在调用ROS Service时当参数有多个时存在错误:参数(在传参数的类中被定义成field)的顺序依赖于参数的名字的排序,而不是你在传参数的类中定义的field的顺序,另外,把类的序列化版本号也加入到list中了。
首先看RosBridge Protocol v2.0关于call service的格式定义:
{ "op": "call_service",
(optional) "id": <string>,
"service": <string>,
(optional) "args": <list<json>>,
(optional) "fragment_size": <int>,
(optional) "compression": <string>
}
显然,如果调用service时有参数的话,参数需要集合list形式组织作为args的值,RosBridgeClient把它处理为参数值的list,而不是真正的list<json>,即不是json键值对形式,这点真是有点扯!举个例子就清楚了:
{"id":"24","op":"call_service","service":"FaceDetectService","args":[200,"ml.svm"]}
从上面可见,参数传递时只传递参数值,不需要参数的名字,参数值以list形式组织,这里的list格式其实就是对应于python的list数据类型,ROSBridge Server就是用python写的,收到json数据后解析出args后就可以很方便转换为list使用。这里参数值的配列顺序需要按照你定义并实现的ROS Service的.srv文件中定义的参数顺序来,也就是参数的数据类型需要一一对应,不能错乱,否则调用时收到的响应数据中会有ROS报的类似如下的错:
{"id": "0", "values": "CamerServices/FaceDetectServiceRequest message requires a int32 for field count, but got a <type 'unicode'>", "result": false, "service": "FaceDetectService", "op": "service_response"}
原因就是你通过RosBridgeClient传的参数的数据类型(string)与FaceDetectServiceRequest (执行catkin_make时根据srv文件生成的)里定义的对应位置的参数的数据类型(int32)不一致,如果只有一个参数,一般不会出现这样的问题,如果有多个参数,你又没注意到ROSBridgeClient的缺陷,十有八九会遇到这个错误。
结合我自己写的代码和ROSBridgeClient的源码,看ROSBridgeClient是如何组织call_service的参数的:
首先ROS端定义了一个FaceDetectService.srv:
int32 count
string reqStr
---
string respStr
在CMakeLists.txt中包含它:
add_service_files(
FILES
FaceDetectService.srv
)
在serivce的实现源码文件中包含编译时将生成的头文件FaceDetectService.h并做service相关代码实现:
int main(int argc, char ** argv)
{
ros::init(argc,argv,"face_detect_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("FaceDetectService",detectFace);
ROS_INFO("Ready to call FaceDetectService...");
ros::spin();
return 0;
}
bool detectFace(camerservices::FaceDetectService::Request & req,camerservices::FaceDetectService::Response & res)
{
ROS_INFO("req.reqStr=%s,count=%d",req.reqStr.c_str(),req.count);
...
std::string resStr = "{\"found\":true}";
res.respStr = resStr;
return true;
}
ROSBridgeClient端定义对应的Request和Response类:
public class FaceDetectServiceRequest extends Message {
public String reqStr="{ml.yolo}"; //default algorithm model
public int count=200;
public FaceDetectServiceRequest(String reqStr,int count){
this.reqStr = reqStr;
this.count= count;
}
}
public class FaceDetectServiceResponse extends Message {
public String respStr;
}
注意这里Request和Response类内参数定义的顺序不一定需要和FaceDetectService.srv里参数定义的顺序一致,但是参数的名字非常重要!参数名字在排序上要使得顺序和FaceDetectService.srv里参数定义的顺序一致!上面reqStr和count的定义顺序与FaceDetectService.srv里参数定义的顺序并不一致,但是参数名count在排序时排在reqStr的前面,ROSBridgeClient在把参数值组织成list时是按参数的名字排序的:
{"id":"24","op":"call_service","service":"FaceDetectService","args":[200,"ml.yolo"]}
这样就使得顺序与FaceDetectService.srv里参数定义的顺序一致了!我们看相关代码就明白了:
我写的Client端service调用的代码:
Service<FaceDetectServiceRequest, FaceDetectServiceResponse> faceDetectionService =
new Service<FaceDetectServiceRequest, FaceDetectServiceResponse>("FaceDetectService",
FaceDetectServiceRequest.class, FaceDetectServiceResponse.class, client);
String fdResult = faceDetectionService.callBlocking(new FaceDetectServiceRequest()).respStr; //call default mode
根据调用关系往下看ROSBridgeClient的相关源码:
public class Service<CallType extends Message, ResponseType extends Message> extends Message implements FullMessageHandler {
...
public ResponseType callBlocking(CallType args) throws InterruptedException {
return take(call(args));
}
public String call(CallType args) {
return callImpl(args, null);
}
private String callImpl(CallType args, MessageHandler<ResponseType> responseHandler) {
client.register(ServiceResponse.class, service, responseType, this); // do this once on creation?
CallService messageCallService = new CallService(service, args);
String id = messageCallService.id;
CallRecord callRecord = new CallRecord(responseHandler);
calls.put(id, callRecord);
client.send(messageCallService);
return id;
}
public class CallService extends Operation {
@Indicator public String service;
@Indicated @AsArray public Message args;
public Integer fragment_size; // use Integer for optional items
public String compression;
public CallService() {}
public CallService(String service, Message args) {
this.service = service;
this.args = args;
}
}
public class ROSBridgeClient extends ROSClient {
...
public void send(Operation operation) {
client.send(operation);
}
...
}
public class ROSBridgeWebSocketClient extends WebSocketClient {
...
public void send(Operation operation) {
String json = operation.toJSON();
if (debug) System.out.println("ROS> " + json);
send(json); //发往RosBridge Websocket Server
}
...
}
public class Operation extends Message {
...
public String toJSON() {
return JSONUtil.toJSON(this);
}
...
}
public class JSONUtil {
...
public static String toJSON(Message m) {
JSONObject jo = convertObjectToJSONObject(m); // Object to JSON-Simple
return jo.toJSONString(); // JSON-Simple to string
}
private static JSONObject convertObjectToJSONObject(Object o) {
JSONObject result = new JSONObject();
for (Field f : o.getClass().getFields()) {
Object fieldObject = getFieldObject(f, o);
if (fieldObject != null) {
Object resultObject;
if (Indication.isBase64Encoded(f))
resultObject = convertByteArrayToBase64JSONString(fieldObject);
else if (Indication.asArray(f))
resultObject = convertObjectToJSONArray(fieldObject);
else resultObject = convertElementToJSON(fieldObject);
result.put(f.getName(), resultObject);
}
}
return result;
}
private static JSONArray convertObjectToJSONArray(Object o) {
JSONArray result = new JSONArray();
for (Field f : o.getClass().getFields()) {
Object fieldObject = getFieldObject(f, o);
if (fieldObject != null && !f.getName().equalsIgnoreCase("serialVersionUID")) {//这里需要过滤掉serialVersionUID !!!
Object resultObject = convertElementToJSON(fieldObject);
result.add(resultObject);
}
}
return result;
}
从上面的代码可以看出,ROSBridgeClient在把数据发往ROSBridge Websocket Server前依照ROSBridge Protocol v2.0规范做了序列化,JSONUtil(原来的名字叫JSON,我上面提到过,觉得在使用快捷键导入时经常跟支持包里的JSON类冲突不好用,而且命名不准确,其实应该是个Util类,就把它改名叫JSONUtil了)在把Operation类型的实例序列化成json字符串时使用的
for (Field f : o.getClass().getFields())
遍历类的所有field,getFields()得到的是个按名字排序的集合,自然在序列化是也是按此顺序处理的了,在CallService类里args被声明为array,因此调用convertObjectToJSONArray()来处理成JSONArray,这里依旧是使用
for (Field f : o.getClass().getFields())
从Class.getFileds()得到按名字排序的Field的集合然后把每个field的值放到JSONArray result中。
另外,java源码编译成字节码后类中都加了serialVersionUID这个field,RosBridgeClient却没有把它过滤掉!因此上面我修改了一下代码增加了判断"&& !f.getName().equalsIgnoreCase("serialVersionUID")",不然这个编译时生成的field会意外地改变你的参数排序,从而可能使得JSONUtil生成的"args"的list里参数值排序不正确,导致ROS会报上面提到的参数类型错误!,例如
"args":[10,-1493428081299389036,5934359477748456932,"{\"a\":1,\"b\":2}"]
上面很长的数字就是Request类的serialVersionUID field的值,这些fields的值是不应该被加到args list中的!
对于Service Response,稍有差异,ROSBridge Protocol v2.0中定义Service Response为:
{ "op": "service_response",
(optional) "id": <string>,
"service": <string>,
(optional) "values": <list<json>>
}
上面的values就是响应中的参数值也是list集合,ROSBridge Server端在组织返回参数为values时是严格遵循了list<json>的规范,例如:
{"id": "24", "values": {"respStr": "{\"found\":false,\"error\":\"No face is found\"}"}, "result": true, "service": "FaceDetectService", "op": "service_response"}
可见,values中的返回参数是以json键值对形式组织的,不存在排序的问题。
更多推荐
所有评论(0)