play with ros 2

Posted on |

ros msgs

ros msgs usually used in C++, as our ADS data tool is implemented in Python, I’d try to build *.msg to python module. find two blogs from ROS doc:

writing a ROS python Makefile

create a ros msg

to build msg into python module, the package.yml should at least include the following lines:

1
2
3
4
5
6
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>

the default CMakeList.txt looks like: 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
find_package(catkin REQUIED COMPONENTS
	roscpp
	rospy
	std_msgs
	message_generation
) 
add_message_files(
	FILES
	custom.msg
)
generate_messages(
	DEPENDENCIES
	std_msgs
)

the generated msg python module is located at ~/catkin_ws/devel/lib/python2.7/dist-packages/my_msg_py/msg,which can add to $PYTHONPATH for later usage

create ros node with catkin

first check your $ROS_PAKCAGE_PATH, the default pkg path is /opt/ros/kinetic/share, append custom pkgs path from ~/cakin_ws/devel/share. 

1
2
3
cd ~/my_catkin_ws/src
catkin_create_pkg my_pkg [dependencies, e.g. sd_msgs rospy]
rospack find

catkin_create_pkg will create a CMakeList.txt at pkg level, and a src folder, where can hold custom nodes definition.

sensor serial data to ros node

sensors(e.g. rtk, imu) to ros is communication from external world to ros sys. Things need to take care: mostly sensor hardware device doesn’t support ROS driver directly, so first need device serial or CAN or Ethernet to get the raw sensor data, and package it as sensor/raw_msg to publish out; the real ros-defined sensor node, will subscribe sensor/raw_msg and publish the repackaged sensor/data to the ros system, (which usually happened in ros callback).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
def rtk_cb(std_msgs::ByteMultiArray raw_msg):
    rtk_msg = func(raw_msg)
    pub.publish(rtk_msg)
pub = nodeHandler.advertise<sensor_msgs:NavSatFix>("/rtk_gps/data", 1)
sub = nodeHandler.subscribe("/rtk_gps/raw_data", 1, rtk_cb)
def raw_data_generator():
	try:	
		with open("/dev/ttyS0", "r|w") as fd:
			header = read(fd, buf, header_line)
			while ros::ok():
				content = read(fd, buf, content_lines)
				std::msgs::ByteMultiArray raw_msg 
				raw_msg.data.push_back(header)
				raw_msg.data.push_back(content)	
				pub.publish(raw_msg)
		close(fd)
	except:
		print("failed to read raw data\n")

sensor CAN data to ros node

sensors(such as camera, radar, lidar e.t.c) go to ros sys through Veh CAN Bus. the difference between CAN msg and serial msg is data atomicity. as serial msg is only one variable, which gurantee atomicity in application level; while each CAN frame usually include a few variables, which need custom implement atomicity in application level. 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
thread0 = pthread_create(recv_thread, raw_can_data)
thread1 = pthread_create(send_thread)
def thread0():
 	 recv_data = func(raw_can_data)
	 pthread_mutex_lock(mutex_lock)
	 sensor_raw = recv_data 
	 sem_post(sem_0)
	 pthread_mutex_unlock(mutex_lock)
def thread1():
	pthread_mutex_lock(mutex_lock)
	sensor_ros_msg =  sensor_raw 
	pthread_mutex_unlock(mutex_lock)
	node_.publish(sensor_ros_msg)

ros node to external device

another kind of communication, is from ros system to external device/env, such as dSPACE. the external device, if not communication through serial, then usually support Ethernet(udp/tcp), which then need to implement a custom udp/tcp data recv/send func. the ros node subscribe the necessary data, then send it out through udp/tcp.

1
2
3
4
5
def cb(sensor_msg):
	data = repack(sensor_msg)
	udp.send(data)
sub = nodeHandler.subscribe("/useful/data", 1, cb)

xml-rpc && tcpros

the ros sytem has two communication, to register/update ros node, publish/subscribe topics to ros master. this kind of message go through xml-rpc. after worker nodes registered in master node, the P2P communication can generated, and the data is transfered through tcpros.

each ros node has a xml-rpc server, in code, nodeHandler.advertise() called in publisher/subscriber node, to register their topices to ros master.

once a subscribe node register to master, which topics it subscribed, master returns a URI as response, then the subscriber and publisher can build connection through this URI. when a publish node register to master, master call publisherUpdate() to notify all subscriber, who subscribe topices from this publisher.

ros visual(rviz)

ros-rviz

  • how rviz works ?

If you want to create a node providing a set of interactive markers, you need to instantiate an InteractiveMarkerServer object. This will handle the connection to the client (usually RViz) and make sure that all changes you make are being transmitted and that your application is being notified of all the actions the user performs on the interactive markers.

image

  • rviz rosbag

rviz config can customize the rviz display, the default located at ~/.rviz/default.rviz.

the idea to play rosbag and render in rviz is to define a custom node, to receive the custom pkg_msg from replayed rosbag, then repckage pkg_msg as corresponded marker/markerArray, then publish these msg out, which will be received by rviz

usually we define a custom node to receive replayed topics from rosbag.play(), and define a callback func to publish its marker objects out. 

1
2
3
4
5
6
7
8
9
10
ros::Publisher markerArray 
def pkg_cb(sensor_pkg):
	for objIdx in sensor_pkg.ObjNum:
		prepare_marker(marker, sensor_pkg.objects[objIdx]
		SensorDisplay.markers.append(marker)	
	markerArray.publish(SensorDisplay)
	SensorDisplay->markers.clear()
subPkg = nodeHandler.subscribe("sensor_pkg", 1, pkg_cb);
markerArray = nodeHandler.advertise<visulization_msgs::MarkerArray>("sensor_pkg", 1)

ros summary

ros is a very common communciation way and message type in ADS dev, many demo are implemented based on ros, which gives a bunch of ros related tools. in this blog, we review three of them:

  • ros based sensor device data collection
  • ros rviz
  • rosbag.play

which can support a few kinds of applications, e.g. debuging, replay, data collection.