我的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) {     

扫描二维码关注公众号，回复： 10784248 查看本文章

    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键值对形式组织的，不存在排序的问题。