我的AI之路(25)--ROSBridge：机器人与外部系统之间的通讯解决方案

     关于ROS的书和网上讲ROS的文章已经很多，绝大多数都是人人亦云的泛泛的讲表面层次的ROS的各部分怎么运用到机器人的各部分功能上、怎么执行命令之类的，对入门当然有用，但缺乏深层次的代码分析或架构层次的结构分析或解决机器人产品某些具体需求的解决方案，对实际做产品/项目开发可借鉴的有限，所以不想再重复说前一方面的知识，而是结合本人参与开发机器人产品中积累的经验侧重讲讲后一方面的知识供分享与交流。

     通常的理解是RosBridge(http://wiki.ros.org/rosbridge_suite)是一个可用于非ros系统和ros系统进行通信的功能包，非ros的系统使用指定数据内容的基于JSON(或BSON)格式的网络请求(ROSBridge支持TCP、UDP、WebSocket三种网络通讯方式)来调用ROS的功能，既然非ROS系统能通过ROSBridge基于TCP/UDP/WebSocket与机器人上的ROS进行交互，那就是实现了外部系统和机器人上的ROS的解耦合，也就是外部系统完全可以与机器人使用不同的开发语言不同的OS平台。 ROS是用在机器人本体上的(前面说过，ROS只是个中间件，跑在机器人内安装的Ubuntu之类Linux上，而且两者的版本安装要匹配，要按ROS官网上指定的来)，而管理和控制机器人的机器人后端(或者叫服务端)控制系统(或者叫平台)通常是使用Java,C#之类的语言开发的，ROSBridge就非常适合用于两者之间的交互通讯实现机器人后端控制系统对机器人的控制(这里的说的控制不仅仅包含机器人遥控器对机器人的那种运动和语音之类的手段控制功能，而且还包括机器人后端控制系统向机器人下发配置数据、地图与导航路径数据、任务数据、特殊动作实时控制、软件更新等等数据以及机器人向机器人后端控制系统上传音视频设备实时获取的音视频数据、地图坐标数据、各种传感器收集的数据、告警数据、任务执行相关数据等等)。另外，我觉得机器人之间也适合使用ROSBridge来进行通讯，无论另外的机器人使用的是Ubuntu+ROS还是纯Android的还是三者都使用了(三者都使用了的情况常见于有人机交互界面采用上下位机两块板子的机器人)。  当然，如果融资多、人力资源充裕、工期不紧张并且有特殊大数据传输要求，完全可以不使用ROSBridge，自己用C++开发一个TCP/WebSocket Sever部署在Linux上，作为机器人本体上的ROS与机器人后端控制系统之间的通讯的桥梁。如果创业公司需要快速出产品、人手又少、没多少钱烧，那么基于现有的ROSBridge来实现机器人后端控制系统与机器人之间的通讯还是比较好的选择。

   ROS默认安装没有包含ROSBridge，需要执行下面命令来安装它：

sudo apt-get install ros-<rosdistro>-rosbridge-suite

比如: 

sudo apt-get install ros-kinetic-rosbridge-suite

(或者也可以从 https://github.com/RobotWebTools/rosbridge_suite 获取到完整的源码包，从下载的包可以看到其实源码都是用python写的脚本，另外是一些msg和srv的定义文件，然后使用catkin编译和安装)

安装完后，可以在/opt/ros/kinetic/lib/python2.7/dist-packages/下看到两个重要的包rosbridge_server和rosbridge_library。

ROSBridge内容包括通讯协议规范定义rosbridge v2.0 Protocol Specification和代码实现。

rosbridge_server、rosbridge_library、rosapi就是ROSBridge的代码实现部分，三部分的功能分别是(直接贴官网的原文，但是把有些分散在多处的描述合并到一起了便于准确理解，不翻译了，不翻译保证细节信息不丢失 ^_^ )：

• rosbridge_library - The core rosbridge package. The rosbridge_library is responsible for taking the JSON string and sending the commands to ROS and vice versa  – the core rosbridge JSON-to-ROS implementation.

• rosbridge_server - While rosbridge_library provides the JSON<->ROS conversion, it leaves the transport layer to others. Rosbridge_server provides a WebSocket connection so browsers can "talk rosbridge." Roslibjs is a JavaScript library for the browser that can talk to ROS via rosbridge_server. It depends on the rosbridge library, and implements the WebSockets server, passing incoming messages to the API and outgoing messages back to the WebSockets connection. The default server uses tornado, a python server implementation.

• rosapi -  Makes certain ROS actions accessible via service calls that are normally reserved for ROS client libraries. This includes getting and setting params, getting topics list, and more.

下面看一下这些代码里的一些主要类的主要功能，以理解ROSBridge的大致结构和工作原理：

1) 源码目录ros_library下的RosbridgeProtocol类(在RosbridgeProtocol.py里)继承自Protocol类(在Protocol.py里)，主要定义了Rosbridge支持哪些功能调用:

 rosbridge_capabilities = [CallService, Advertise, Publish, Subscribe, Defragment, AdvertiseService, ServiceResponse, UnadvertiseService]

在初始化里创建并添加这些功能调用对应的handler类的实例：

for capability_class in self.rosbridge_capabilities:
            self.add_capability(capability_class)

add_capability()是在Protocol类里实现的：

    def add_capability(self, capability_class):

        self.capabilities.append(capability_class(self))

上面的每个capability都继承自Capability类，并且在__init__(self,protocol)函数里调用protocol.register_operation()把自己的处理函数作为opcode对应的handker注册到protocol的operations[]里去，例如CallService类(在capabilities/call_service.py里)的__init__()：

    def __init__(self, protocol):

       Capability.__init__(self, protocol)    

       protocol.register_operation("call_service", self.call_service)

Protocol类的register_operation()：

def register_operation(self, opcode, handler):

   self.operations[opcode] = handler

opcode具体有哪些可能的值以及示例，可参见 https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md里第3节内容。

Protocol类的deserialize()把JSON/BSON格式数据解析到dict里，serialize()则是相反，把dict形式的数据序列化成JSON/BSON格式的数据，incoming()则是调用deserialize()把buffer里收到的JSON/BSON格式数据解析到msg里，并根据msg里的op值调用对应的hanlder来处理这个调用：

def incoming(self, message_string=""):

   msg = self.deserialize(self.buffer)

   op = msg["op"]

   self.operations[op](msg)

这里的handler调用rosbridge_library/inernal下的功能实现包装类的对应的方法来调用rospy (ROS的python版的Client API)的API来调用ROS的功能，以CallService类的call_service()方法为例：

def call_service(self, message):

    ServiceCaller(trim_servicename(service), args, s_cb, e_cb).start()

 ServiceCaller是rosbridge_library/inernal/services.py里定义的线程类(继承自threading.Thread类)，ServiceCaller的run()方法:

def run(self):

   self.success(call_service(self.service, self.args))

def call_service(service, args=None):

    service = resolve_name(service)

    ...

    proxy = ServiceProxy(service, service_class)
    response = proxy.call(inst)

    ...    

2) rosbridge_server下的launch目录里有三个启动文件rosbridge_tcp.launch、rosbridge_udp.launch和rosbridge_websocket.launch供执行启动TCP server或UDP Server或WebSocket Server使用，他们执行的分别是scripts目录下的rosbridge_tcp.py、rosbridge_udp.py、rosbridge_websocket.py这个三个python文件(由.launch文件里的node定义中的type指定)，这三个python文件分别启动对应的Server并把src/rosbridge_server目录下的tcp_handler.py、udp_handler.py、websocket_handler.py里分别定义的RosbridgeTcpSocket、RosbridgeUdpSocket、RosbridgeWebSocket三个类分别用作对应通讯方式的handler，对连接的建立/关闭、数据的收/发进行处理。

     关于上面的TCP/UDP/Websocket三种通讯协议的实现该选用哪种好，我觉得这个需要根据你的系统的具体通讯需求来做选择，只需要短连接、可靠、单向、一对一(或者少量一对多)传输数据的话选用TCP即可，只需要单向、非可靠、一对多(一对一当然可以啦)传输数据的话选用UDP就行，对于要求频繁双向传输数据、尤其需要最好保持长连接的通讯需求，当然使用WebSocket是最好的选择。

     十几年前我在J2EE平台上做B/S三层结构系统架构设计时，常为一件事情苦恼，那就是众所周知HTTP 协议是一种短连接、单向的应用层协议，它采用的请求/响应工作模式，通讯过程中，请求只能由客户端发起，服务端对请求做出应答处理，服务端无法主动向客户端推送消息，如果服务器有状态或其他应用数据变化，客户端无法得到通知，当时Web 页面只能采用定时调用AJAX主动去轮询服务端，虽然AJAX能避免整个页面刷新，但是轮询的效率低，轮询周期设置容易顾此失彼造成不同的副作用(做过软件开发的这里不用多说能明白)。后来换公司了转向C/C++语言为主嵌入式开发领域后没关注这些了。

      后来得知，HTML5出现了个叫WebSocket的玩意解决了这个问题，并且2011年的RFC 6455定义了详细的规范。JavaEE 7也推出了JSR-356 Java API for WebSocket规范(也就是定义javax.websocket.*这些API吧)，服务器跟着实现这个JSR规范，Tomcat从7.0.27开始支持 WebSocket，从7.0.47开始支持JSR-356，在安装Tomcat后在lib目录里可以找到websocket-api.jar和tomcat-websocket.jar的包，就是对javax.websocket.*的实现。   如果基于Java做开发，除了Java EE规定的API和实现，还有些第三方的WebSocket的Java实现代码可以使用，比如 https://github.com/TooTallNate/Java-WebSocket ，下载它的release版https://github.com/TooTallNate/Java-WebSocket/releases里最新的1.3.9版的类包Java-Websocket-1.3.9.jar集成到你的系统里也是很好用的，不过这时不要使用javax.websocket.* API了，而是应该使用org.java_websocket.* API。最近开发系统中，两者我都用过，都提供了WebSocket的Server和Client的实现，都比较好用。

    回到rosbridge_server的rosbridge_websocket上来。 机器人系统与外部系统之间的通讯一般都需要双向收发数据，并且经常需要保持长连接，对于这种需求，如果采用传统的TCP/UDP通讯来实现显然麻烦，双方都需要实现有Server和Client，并且需要维持两个连接来实现双向收发，因此采用WebSocket通讯是比较合理的。

启动Rosbridge的WebSocket Server需执行:

roslaunch rosbridge_server rosbridge_websocket.launch

前面说过，执行上面的launch文件会执行rosbridge_websocket.py启动一个WebSocket Server(这个Server是基于python的tornado的，默认端口是9090，可以在rosbridge_websocket.launch配置: <arg name="port" default="9090" /> )并且把websocket_handler.py里定义的RosbridgeWebSocket注册为handler处理连接的建立/关闭、数据的收/发等事件。RosbridgeWebSocket类继承自tornado.websocket.WebSocketHandler(在/usr/lib/python2.7/dist-packages/tornado/websocket.py里)，它重新定义了几个主要方法用于上述事件的处理:

def open(self):

   self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters)

...

def on_close(self):

...

def on_message(self, message):

...

     self.protocol.incoming(message)

...

def send_message(self, message):

...

    上面最重要的方法就是on_message()，它调用Protocol的incoming()以解析收到的JSON/BSON数据并根据op值调用对应的rosbridge_library/capbilities里的Capability子类的方法，进而调用ROS的功能API。

     从上面可以看到，RosBridge WebSocket Server启动后在端口9090监听，外部系统可使用WebSocket Client创建一个WebSocket连接连到它上面，并且给它发送遵循rosbridge v2.0 Protocol Specification的JSON/BSON格式的数据，它就会对请求数据进行解析并调用对应的ROS API实现对机器人的控制，比如下面的数据是向/cmd_vel_mux/input/teleop主题发布一个twist消息控制机器人沿X轴方向向前移动一下：

       {

         "op":"publish",

         "id":"1",

          "topic":"/cmd_vel_mux/input/teleop",

           "msg":{

                 "linear": {"x":1.0,"y": 0.0,"z": 0.0},

                 "angular": {"x": 0.0,"y": 0.0,"z": 0.0}

             }

        }

当机器人有数据需要向外部系统发送时，使用同一WebSocket连接向外部系统主动发送数据。

 

下面说说某些问题及解决办法：

     上面的知识，比如上面这个JSON格式的请求命令数据，涉及到ROS和机器人的一些知识，如果外部系统的开发人员做开发时都需要用到跟机器人通讯的功能，每个人都需要对这些有了解并做编码的话显然是不现实的也是不合理的。如果把这些与机器人通讯的功能部分放在外部系统一个公用的模块里实现，其他业务模块需要和机器人通讯时调用这模块封装的API来实现与机器人通讯，这样做比较合理点，因为ROS相关知识只需要做这个模块开发的人知道，与ROS (通过ROSBridge WebSocket Server)交互的实现细节也被限制在一个模块内部。

     另外,如果同一机器人后端控制系统需要和不同种类的机器人通讯，而这些机器人使用的通讯协议不一样，这样在机器人后端控制系统上实现针对不同的机器人的不同的通讯方式的实现和管理实在是麻烦，这时可以考虑在机器人本体上的Server的入口处做些定制改造，以便使机器人后端控制系统能使用一种与各种机器人的通讯协议无关的应用层级的自定义协议与机器人进行通讯，比如，可以在RosbridgeWebSocket类的on_message()里增加代码对收到的来自后端控制系统的自定义应用协议数据做转换，转换成遵循rosbridge v2.0 Protocol Specification的JSON/BSON格式的数据，然后调用self.protocol.incoming(message)。

      一般的op例如Advertise、Publish之类都只是单向调用ROS，无需等待响应/结果数据，那么只需在调用self.protocol.incoming(message)之前对请求数据作转换成符合规范要求的JSON/BSON数据到message即可，对于需要接收来自ROS的响应数据的op，需要在获取并发送回response数据的地方(self.protocol.send(outgoing_msg))的前面增加代码做转换处理(把outgoing_message数据转换成机器人后端控制系统使用的自定义应用协议要求的格式)，例如对于op是"call_service"的调用，可以修改call_service.py里CallService类的方法 _success(self, cid, service, fragment_size, compression, message)和 _failure(self, cid, service, exc)的代码，对于op是"Subscribe"的调用，则可以修改subcribe.py里Subscribe类的方法publish(self, topic, message, fragment_size=None, compression="none")。为什么最好是在这些地方截取response数据并做转换，可以自己分析相关代码的调用关系，不再赘述 。 当然，在这些地方做修改(增加代码)时需要小心，注意不要影响RosBridge代码本身的原始的调用逻辑关系，保证在你的使用自定义应用协议的控制系统能与机器人通讯的同时，使用遵循rosbridge v2.0 Protocol Specification的协议的其他外部系统也能与机器人进行通讯。

     另外，需要特别指出的是，RosBridge只实现了TCP/UDP/WebSocket Server端，没有实现对应的Client端，也就是需要你的机器人后端控制系统或者其他外部系统主动向RosBridge的Server端口(比如WebSocket server的9090)发起连接，RosBridge不具备主动向外部系统的Server发起连接的功能，如果需要机器人在启动后自动主动去连接指定的机器人后端控制系统等外部系统，那么需要你自己实现代码或者安装一个现成的client框架并基于框架写机器人启动时连接到外部系统的代码，比如，如果机器人与外部系统之间需要使用WebSocket连接，那么可以安装一个python版的websocket-client，安装python版websocket-client的命令:

#可选： sudo apt-get install python-pip python-dev build-essential

sudo pip install websocket-client

安装完后可以看到/usr/local/lib/python2.7/dist-packages/下有一个websocket目录，里面的 _core.py负责收发数据。

    然后使用catkin_create_pkg在catkin_ws/src/下创建一个包，比如叫platform_init_conn，然后在这个platform_init_conn包的script目录里基于websocket-client写一个python脚本platform_init_conn.py，从node 读取Parameters并连接到外部系统，然后根据需要创建topic用于接收其他节点的消息等等功能，示例代码如下：

import websocket

    def send_recv(self,req_msg,url):
        ws = websocket.WebSocket()
        ws.connect(url)
        ws.send(req_msg)
        resp_msg = ws.recv()
        #print resp_msg
        ws.close()
        ws.shutdown()  #close and release each time
        return resp_msg

 在launch目录下为其配置一个platform_init_conn.launch文件,在platform_init_conn.launch文件里配置外部系统的IP和端口等配置参数并定义一个node以执行platform_init_conn.py(注意授予platform_init_conn.py的可执行权限，否则执行roslaunch时可能报错找不到文件)，然后把roslaunch platform_init_conn platform_init_conn.launch加到Linux的 ~/.bashrc里去以便在机器人启动时执行这个命令以主动连接到外部系统。

 

     对于ROS的代码，为节省篇幅只列出了解说需要的关键代码；另外，上面有些自己实现的代码有可能涉及到工作中的商业秘密，所以只贴出少量与具体项目秘密无关的代码用于解说。

     

    至于调用ROS Client Library也能与ROS交互为什么非要使用RosBridge，我觉得原因有这么几个：

     1. ROS Client Library中的c++、python、java、c#等几个版本在目前最新版的ROS中只有ros cpp、rospy还在维护支持；     

     2. ROS Client Libraries let you write ROS nodes, publish and subscribe to topics, write and call services, and use the Parameter Server，看到了吧，这相当于把ROS用不同语言做了重量级实现，如果你选择使用其中某种Client，就选择了耦合紧密的实现，编写调用这些Client的API的人需要熟悉ROS。

     3.调用Client API可以看成是本地访问调用ROS，虽然ROS的Master node和一般的node可以分别部署在不同的机器上，但是它们之间的相互通讯是ROS内部特定的通讯方式；基于RosBridge实现的外部系统对ROS的调用实现了解耦合，具有很大灵活性，外部系统只需支持WebSocket之类的通讯协议即可，无需非要实现ROS的node。