Tuesday, May 31, 2011

First exercise with ROS: a node to interface with Player

I am starting to learn ROS. I needed an exercise and I do not have any robot to play with, except Player/Stage. So I decided to write a ROS node for my Player/Stage robot. ROS already has a stack called stageros that allows to interface with Stage. Mine is different: it allows to interface with Player. Hence it can also be used to interface with Stage (via Player), but with any other Player-enabled robot as well. Anyway, it's not very useful, but it's a good experimentation platform and exercise.

Requirements
- Linux (I use Debian Wheezy)
- ROS (see ROS tutorials on how to install and learn the basics)
- Player and Stage (see respective tutorials as well, mine are install from source)
- A Player configuration file and world to start a robot in Stage

I assume familiarity with ROS and Player/Stage

Design
The final aim is to be able to use the navigation stack with the Player robot. So I followed the guidelines and how to setup a robot for the navigation stack:
http://www.ros.org/wiki/navigation/Tutorials/RobotSetup

This means that we will need to publish laser data on the base_scan topic, odometry data on the odom topic, publish transform configuration too, and accept velocity commands on the cmd_vel topic.

I wrote 2 versions. One version has everything in one node, the other version is made of 3 smaller nodes (one per task aforementioned). We will first check the one node version.

The code is object oriented. I found the solution better than having global variables, because it allowed better control of initialization order: some stuffs can only be created after ros::init has been called.

Preparation
First of all, let's create a package.

I have created a directory where I can experiment, i.e. ~/ros_playground. I added this directory to the ROS_PACKAGE_PATH environment variable: in ~/.bashrc, after the source xxx/ros/setup.sh line added when installing ROS (see ROS tutorial), I added the following line:
ROS_PACKAGE_PATH=~/ros_playground:$ROS_PACKAGE_PATH

Then from the ros_playground directory:

roscreate-pkg playerros_1node roscpp tf nav_msgs sensor_msgs geometry_msgs

playerros_1node
is the name of my package. The roscreate-pkg creates a directory ~/ros_playground/playerros_1node that contains several files, including the CMakeList.txt and manifest.xml

roscpp
, tf, nav_msgs, sensor_msgs and geometry_msgs are the dependencies we will need. roscpp is needed to write a C++ ROS client, tf is needed to send transform configurations, nav_msgs for the odometry messages, sensor_msgs for the laser messages, and geometry_msgs for the velocity command messages.


Code for the one node version
Create playerros.cpp in the src directory in the playerror_1node directory just created.

First the includes:


Then the class definition:


The constructor creates all the Player objects, connects to Player, sets the callback functions (when new laser or odometry data is available), sets the cmd_vel callback function when a cmd_vel message is received from another ROS node, and starts the Player thread. We will look at all of this in detail, but first the main function:



Up to here, I think everything should be quite obvious. Let's now take a look at the contructor:


Now the odometry callback function:


One note here on frames. This code defines a few frames. One is called base_link, which is the root frame of the robot, i.e. the base. The odom frame is a fixed frame; it's origin is defined by the starting position of the robot. There is also the base_link frame, which is the frame attached to the laser.




Full code: http://pastebin.com/psZyY40E

Building the code
Edit CMakeList.txt:


To build:
rosmake playerros_1node
This will compile all the dependencies and the code we just created.
In case there are errors and you just want to recompile player_node.cpp, simply type make

Running
rosrun playerros_1node playerros_node
(Player must be running first)

Testing
The program can be tested by sending commands to the robot. This can be done using rostopic:

rostopic pub cmd_vel geometry_msgs/Twist '[0.2,0,0]' '[0,0,0]' -1

This will set the velocity to 0.2m/s

Or you can use the following program to control the robot from the keyboard.
http://pastebin.com/7ibrBQq8

Add the following line to CMakeList.txt to compile:
rosbuild_add_executable(teleop src/teleop.cpp)