[PYTHON] Bridge ROS to your own protocol

2019/02/11 postscript

It seems that you are at the top of the Japanese information that comes out by searching for rosbridge, so please advertise a little.

The company I belong to, aptpod, is a company that develops PubSub brokers using WebSocket in-house (for a little more detailed explanation, Advent Calendar. I wrote in 551f5ad60d8c4cd14588)), the following article was actually the content I searched for for the connection to the company's PubSub broker (although the article says it is the connection to Socket.io).

Recently, it seems that some customers want to visualize the ROS data of robots remotely, so if you have any problems in transmitting and visualizing large volumes of data (not limited to ROS), please do not hesitate to contact us. Please contact us at (https://www.aptpod.co.jp/).

Introduction

There is a ROS module called Rosbridge that bridges ROS messages to other protocols. The rosbridge_server included in the package has a server program that uses UDP, TCP, and WebSocket as transport protocols. Especially for WebSocket, you can ROS your browser very easily by using roslibjs, which is a js library. You can participate in.

rosbridge_suite - ROS Wiki

This time, in order to bridge rosbridge to a protocol other than the protocol included in the package (Socket.io this time) such as UDP, TCP, WebSocket, let's consider a method of directly using the Rosbridge protocol (rosbridge_library). By using the Rosbridge protocol, you don't have to implement it for each operation type such as pubsidh / subscribe / service ..., and you don't have to import the module of each message, so you can bridge very easily. I will.

There are still few Japanese materials about ROS, so I hope it helps someone.

Assumption

--Assuming another Socket.io server is running --Connect to the above server and bridge the messages sent from the server to ROS --Bridge the received ROS message to the above server

Implementation

Overview

--Instantiate the Rosbridge Protocol --Execute RosbridgeProtocol.incoming in the incoming callback function of the external protocol --Execute the send function of the external protocol in RosbridgeProtocol.outgoing

Source code

mybridge.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import json
import rospy
import socketIO_client
from rosbridge_library.rosbridge_protocol import RosbridgeProtocol

# ############################
# arguments
# ############################
host = sys.argv[1]
port = int(sys.argv[2])


# ############################
# init RosBridgeProtocol
# ############################
rospy.init_node('mybridge')
client_id_seed = 0;
protocol = RosbridgeProtocol(client_id_seed)


# ############################
# init Socket.io
# ############################
socketIO = socketIO_client.SocketIO(host, port)

def on_connect(*args):
    rospy.loginfo('socket.io connected.')

def on_disconnect(*args):
    rospy.loginfo('socket.io disconnected.')

socketIO.on('connect',    on_connect)
socketIO.on('disconnect', on_disconnect)


# ############################
# on websocket message
# ############################
def on_message(*args):
    message = json.loads(args[0])
    protocol.incoming(message)

socketIO.on('ws-receive', on_message)


# ############################
# on ros message
# ############################
def outgoing_func(message):
    msg = json.loads(message)
    socketIO.emit('ws-send', json.dumps(msg))

protocol.outgoing = outgoing_func


# ############################
# start Socket.io
# ############################
socketIO.wait()

Run

$ python mybridge.py 192.168.0.1 80

Code commentary

Initialization of Rosbridge Protocol

rospy.init_node('mybridge')
client_id_seed = 0;
protocol = RosbridgeProtocol(client_id_seed)

--I don't call it, but if you don't run rospy.init_node, you will get an error. --I'm not sure about client_id_seed, but 0 seems to be fine (the included server program also uses 0).

SocketIO-Client initialization

socketIO = socketIO_client.SocketIO(host, port)

def on_connect(*args):
    rospy.loginfo('socket.io connected.')

def on_disconnect(*args):
    rospy.loginfo('socket.io disconnected.')

socketIO.on('connect', on_connect)
socketIO.on('disconnect', on_disconnect)

Initializing SocketIO-Client. I haven't done anything special.

Execute ʻincoming` in the incoming callback of SocketIO-Client

def on_message(*args):
    message = json.loads(args[0])
    protocol.incoming(message)

socketIO.on('ws-receive', on_message)

--Call ʻincoming in the callback ʻon_message when the'ws-receive' event is received by SocketIO-Client. --ʻIncomingcan be converted to a ROS message and sent by passingmessage` according to ROSBRIDGE_PROTOCOL.md. Will do it.

ʻExecute SocketIO-Client send function in outgoing`

def outgoing_func(message):
    msg = json.loads(message)
    socketIO.emit('ws-send', json.dumps(obj))

protocol.outgoing = outgoing_func

--Override ʻoutgoing in Rosbridge Protocol. --ʻOutgoing is a function that is called every time a ROS message is received. --message is a JSON string that follows ROSBRIDGE_PROTOCOL.md. --Send as a 'ws-send' event from SocketIO-Client.

socketIO.wait()

Wait for SocketIO-Client to receive. For Rosbridge Protocol, there is no need to execute a function for waiting.

Rosbridge protocol

Basically, it is OK if you send a message according to ROSBRIDGE_PROTOCOL.md. An example is shown below, but please refer to the link source for detailed options.

publish If you want to publish, send the following message from the server side. The sample below is an example of the topic / cmd_vel of type geometry_msgs / Twist.

{
  "op": "publish",
  "topic": "/cmd_vel",
  "msg": {
    "linear"  : { "x" : 0, "y" : 0, "z" : 0 },
    "angular" : { "x" : 0, "y" : 0, "z" : 0 },
  }, 
}

subscribe If you want to subscribe, send the following message from the server side to start bridging the message. Below is an example of the topic / odom. Just the topic name is OK.

{
  "op": "subscribe",
  "topic": "/odom",
}

To unsubscribe a message once subscribed, do as follows.

{
  "op": "unsubscribe",
  "topic": "/odom",
}