rosapi
说明:
介绍rosapi各文件及功能
代码目录结构:
├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ └── TypeDef.msg #消息类型定义 ├── package.xml ├── scripts #脚本 │ └── rosapi_node #初始化服务和参数 ├── setup.py ├── src │ └── rosapi #API具体实现 │ ├── __init__.py │ ├── objectutils.py │ ├── params.py │ └── proxy.py └── srv #定义各种服务 ├── DeleteParam.srv ├── GetActionServers.srv ├── GetParamNames.srv ├── GetParam.srv ├── GetTime.srv ├── HasParam.srv ├── MessageDetails.srv ├── NodeDetails.srv ├── Nodes.srv ├── Publishers.srv ├── SearchParam.srv ├── ServiceHost.srv ├── ServiceNode.srv ├── ServiceProviders.srv ├── ServiceRequestDetails.srv ├── ServiceResponseDetails.srv ├── ServicesForType.srv ├── Services.srv ├── ServiceType.srv ├── SetParam.srv ├── Subscribers.srv ├── TopicsForType.srv ├── Topics.srv └── TopicType.srv
重要文件说明:
scripts/rosapi_node
- 初始化服务
- 服务包含:
rospy.Service('/rosapi/topics', Topics, get_topics) rospy.Service('/rosapi/topics_for_type', TopicsForType, get_topics_for_type) rospy.Service('/rosapi/services', Services, get_services) rospy.Service('/rosapi/services_for_type', ServicesForType, get_services_for_type) rospy.Service('/rosapi/nodes', Nodes, get_nodes) rospy.Service('/rosapi/node_details', NodeDetails, get_node_details) rospy.Service('/rosapi/action_servers', GetActionServers, get_action_servers) rospy.Service('/rosapi/topic_type', TopicType, get_topic_type) rospy.Service('/rosapi/service_type', ServiceType, get_service_type) rospy.Service('/rosapi/publishers', Publishers, get_publishers) rospy.Service('/rosapi/subscribers', Subscribers, get_subscribers) rospy.Service('/rosapi/service_providers', ServiceProviders, get_service_providers) rospy.Service('/rosapi/service_node', ServiceNode, get_service_node) rospy.Service('/rosapi/service_host', ServiceHost, get_service_host) rospy.Service('/rosapi/message_details', MessageDetails, get_message_details) rospy.Service('/rosapi/service_request_details', ServiceRequestDetails, get_service_request_details) rospy.Service('/rosapi/service_response_details', ServiceResponseDetails, get_service_response_details) rospy.Service('/rosapi/set_param', SetParam, set_param) rospy.Service('/rosapi/get_param', GetParam, get_param) rospy.Service('/rosapi/has_param', HasParam, has_param) rospy.Service('/rosapi/search_param', SearchParam, search_param) rospy.Service('/rosapi/delete_param', DeleteParam, delete_param) rospy.Service('/rosapi/get_param_names', GetParamNames, get_param_names) rospy.Service('/rosapi/get_time', GetTime, get_time)
节点说明:
- /rosapi/Topics,返回所有发布的话题列表
- /rosapi/topics_for_type,返回指定类型的所有发布的话题列表
- /rosapi/services, 返回所有发布的服务列表
- /rosapi/services_for_type,返回指定类型的所有发布的服务列表
- /rosapi/nodes,返回所有已经注册的节点列表
- /rosapi/node_details,返回某节点详情
- /rosapi/action_servers,返回action服务列表
- /rosapi/topic_type,通过话题名获取相应的消息类型
- /rosapi/service_type,通过服务名获取相应的消息类型
- /rosapi/publishers,提供话题名获取发布此话题的节点名列表
- /rosapi/subscribers,提供话题名获取接受此话题的节点名列表
- /rosapi/service_providers,提供话题名返回广播此服务类型的节点名列表
- /rosapi/service_node,提供服务名,返回提供此服务的节点名
- /rosapi/service_host,提供服务名,返回提供此服务的主机名
- /rosapi/message_details,提供消息类型名,返回类型的TypeDef
- /rosapi/service_request_details,提供服务类型名,返回消息请求的服务类型的TypeDef
- /rosapi/service_response_details,提供服务类型名,返回消息反馈的服务类型的TypeDef
- /rosapi/set_param,设置参数
- /rosapi/get_param,获取参数
- /rosapi/has_param,判断参数是否存在
- /rosapi/search_param,检索参数
- /rosapi/delete_param,删除参数
- /rosapi/get_param_names,获取参数名
- /rosapi/get_time,获取服务器时间
ROSBridge的工作原理和使用
源码目录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) ...
同时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是最好的选择。
机器人系统与外部系统之间的通讯一般都需要双向收发数据,并且经常需要保持长连接,对于这种需求,如果采用传统的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配置: )并且把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连接向外部系统主动发送数据。