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.
- 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
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:
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.
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:
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
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
rosrun playerros_1node playerros_node
(Player must be running first)
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.
Add the following line to CMakeList.txt to compile: