Controling dynamic reconfiguration from C++ in ROS (roscpp)

ROS has a cool mechanism called Dynamic Reconfigure where parameters of nodes can be set at runtime. The concept is explained here. Sometimes you want to change these parameters from your code. There is a supported python API to do this but there is no API for C/C++ yet.

GUI for dynamic reconfigure which can be launched via rosrun rqt_reconfigure rqt_reconfigure

After some research I found a good thread on the ROS forum here explaining how it is possible use dynamic reconfigure from C++ code in ROS.

Based on that thread I got the dynamic reconfigure working. I wrote this blogpost to take you through the steps needed. Hope this helps you.

In my case, the reason I needed this was that our project is using ar_track_alvar to scan for AR tags. However, this uses a tremendous amount of CPU so I want to be able to switch if off when it's not needed. Alternatively, and arguably better, I could have also modified the ar_track_alvar package when nobody is subscribed to any of the topics. Maybe I'll look into that sometime. But for now, let's dive in.

Nodes that support dynamic reconfigure have a ros service that allows one to set it. Via the command rosservice list you can get a list of those services. In my case it revealed the service that I needed:

/ar_track_alver/set_parameters

We will be invoking this service via C++.

If we run the command rosservice info /ar_track_alvar/set_parameters we can see information about this service:

Node: /ar_track_alvar
URI: rosrpc://ubuntu:42524
Type: dynamic_reconfigure/Reconfigure
Args: config

The argument that we can pass to this service is called 'config'. This is parameter of type dynamic_reconfigure::Config and contains all the parameters that the node allows to be set. To find out more about its internal structure we can look at some of the topics published. Via rostopic list we can quickly find out that there are 2 interesting topics that we can look at:

/ar_track_alvar/parameter_descriptions
/ar_track_alvar/parameter_updates


The first is of type dynamic_reconfigure/ConfigDescription and the latter is of type dynamic_reconfigure/Config.

To find out the parameters that you can set, you can simply look at the content of the topic /ar_track_alvar/parameter_updates via the command rostopic echo /ar_track_alvar/parameter_updates. For this node, the output will look like:

bools:
  -
    name: enabled
    value: True
ints: []
strs: []
doubles:
  -
    name: max_frequency
    value: 10.0
  -
    name: marker_size
    value: 5.5
  -
    name: max_new_marker_error
    value: 0.05
  -
    name: max_track_error
    value: 0.05
groups:
  -
    name: Default
    state: True
    id: 0
    parent: 0
 

Here we can see the boolean parameter 'enabled' that we need to set. Now we have all the information needed to write the code. This is actually pretty straightforward. As we need to set a boolean parameter we use the include file BoolParameter.h. There are corresponding include files for the other elementary data types (IntParameter.h, DoubleParameter.h, etc).

#include <dynamic_reconfigure/BoolParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h>

.
.
.

void ApproachMarker::EnableArTrackAlvar(bool enable) {
  dynamic_reconfigure::ReconfigureRequest srv_req;
  dynamic_reconfigure::ReconfigureResponse srv_resp;
  dynamic_reconfigure::BoolParameter enable_param;
  dynamic_reconfigure::Config conf;

  enable_param.name = "enabled";
  enable_param.value = enable;
  conf.bools.push_back(enable_param);

  srv_req.config = conf;

  if (ros::service::call("/ar_track_alvar/set_parameters", srv_req, srv_resp)) {
    ROS_INFO("call to set ar_track_alvar parameters succeeded");
  } else {
    ROS_INFO("call to set ar_track_alvar parameters failed");
  }

}


Alright, that's it folks. Hope this was of use!

The best webcam on ROS indigo

It is pretty hard to find a good list of 'known good' cameras on ROS.
However, as ROS supports UVC (USB video class) searching for a camera that supports this is great.

Also, a wide angle camera is a success factor to consider.
For many applications have a large field of view is perfect.

Testing cameras
a simple way to test cameras is using guvcview.
syntax:

guvcview
OR
guvcview -d /dev/video1  (or whichever device id the camera is connected to)

you can install guvcview with sudo apt-get install guvcview

Of course, you can also test the camera with the ROS usb_cam package as described in this post.

Cameras tested

• Logitech C270
• Logitech HD C615
• Logitech HD C910
• Logitech HD C920
• Microsoft Lifecam Studio - Works with usb_cam but gives extremely bright images.   Functionally unusable.  It is labelled a widescreen camera and the field of view is almost as good as the Logitech C910
• Media-tech MT4047 - Camera requires a different pixelformat setting (yuyv instead of mjpeg). However, after a few frames the camera seems to freeze
• G-Cube compactcam

My personal absolute favorite is the Logitech HD C920. It is light sensitive, gives a clear picture and largest field-of-view of all cameras tested by me.

Field of view comparison

Simulate your turtlebot in Gazebo

Getting Gazebo up-and-running is described well on the ROS wiki. Here is a short summary.


Step 1: install turtlebot simulator
sudo apt-get install ros-indigo-turtlebot-simulator

Step 2: start gazebo
roslaunch turtlebot_gazebo turtlebot_world.launch


Step 3: start rviz
roslaunch turtlebot_rviz_launchers view_robot.launch


Step 4: launch your other nodes 
For example you can launch the teleop to control your turtlebot and drive it around in the simulator using:
roslaunch turtlebot_teleop keyboard_teleop.launch

Note on world files:
Gazebo can be launched with the world file as parameter.
they can be relative to the existing path, an absolute path or be located in /usr/share/gazebo-<version>/
a subfolder /worlds actually contains a few interesting ones

Stability
Gazebo is not the most stable product. ROS indigo comes with version 2. ROS jade comes with version 5. While ROS jade is available at the time of writing this post, the Turtlebot packages are not. I am axiously looking out for the Turtlebot packages for ROS jade so that I can see if Gazebo indeed deliveres the expected stability improvement.

Gazebo started ok on my machine but gave segmentation faults (exit code 139) later on. I found that this could be due to URDF parsing problems. The following line solved that for me:
export LD_LIBRARY_PATH="/usr/lib/x86_64-linux-gnu/:$LD_LIBRARY_PATH"

Extra models in Gazebo
You can add extra elements to the Gazebo world. Gazebo has an option to import models from an online repository. You can also download this repository locally using :
hg clone https://bitbucket.org/osrf/gazebo_models

Webcam on ROS

For many applications having a webcam running on ROS makes a lot of sense. Getting it running is not very difficult. As always, there are some choices to make.

On ROS indigo, the default driver is libuvc_camera but this is not the best one. The package uvc_cam used to be excellent but is no longer maintained. The best package I know of is usb_cam which I'll be using here.

Here is how to get it working in five simple steps:
Step 1: 
in a catkin workspace / src folder do:
git clone https://github.com/bosch-ros-pkg/usb_cam.git

Step 2:
then run catkin_make from the catkin workspace root folder.

Step 3:
Make sure the <catkin_workspace>/devel/setup.bash file is sourced.

Step 4:
Create a .launch file with the following contents:
<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video1" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="mjpeg" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

change the /dev/video1 to /dev/video0 if needed.
Mostly, on a laptop video0 would be the built-in device and video1 would be the external usb webcam.

Step 5:
roslaunch <newly created launchfile>

Getting roll, pitch and yaw from Quaternion orientation as returned by AR marker pose

In my previous post I used the AR_track_alvar package to find AR markers from the video stream.

Now, suppose you want to use the information in the marker to find out its orientation. The topic returns a pose object which contains pose.orientation atttibute. This attibute is of type quaternion revealing four values x,y,z and w.

For my code, it was easier to deal with 'normal' (Euler) angles giving rotations around the 3D axis. Or in other words: roll, pitch and yaw.

Source: wikipedia

With the AR tag the rotation axis are aligned as follows indicated the the image.

Converting between quaternions and roll, pitch, yaw is easily done via the function getRPY(). The code fragment below illustrates how. The sample listens to the ar_pose_marker topic and if a marker is found, converts the coordinates to roll, pitch and yaw and prints them to the console.

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <ar_track_alvar_msgs/AlvarMarkers.h>

void cb(ar_track_alvar_msgs::AlvarMarkers req) {
    if (!req.markers.empty()) {
      tf::Quaternion q(req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w);
      tf::Matrix3x3 m(q);
      double roll, pitch, yaw;
      m.getRPY(roll, pitch, yaw);
      ROS_INFO("roll, pitch, yaw=%1.2f  %1.2f  %1.2f", roll, pitch, yaw);
      // roll  --> rotate around vertical axis
      // pitch --> rotate around horizontal axis
      // yaw   --> rotate around depth axis
    } // if
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "arlistener");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("ar_pose_marker", 1, cb);
  ros::spin();
  return 0;

}

Note: this code assumes -for simplicity- that there is maximum one marker in the screen at the same time. 

Recognize AR tags with ROS

Augmented Reality tags, or AR markers, are a very useful in robotics. They can be used to identify objects or to calibrate the robot's position. 

I experimented with AR_Kinect earlier on, but that package was very instable. Now, a new package has come out, called AR_Track_Alvar that can be used to scan for AR tags.



Software:
I tested this on ROS Indigo.

Hardware:
Just some piece of paper with *one* AR tag on it. I used this example which I extracted from the samples on the wiki AR_Track_Alvar . 

Installation:
Simply install using 
sudo apt-get install ros-indigo-ar-track-alvar

Starting:
The package comes with some launch files for the PR2 which can be used as an example to create your own.
I use a Turtlebot with a Kinect to scan for AR tags and use the following launch file for that; I named it alvar.launch:
<launch>
    <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>

    <arg name="marker_size"          default="5.0" />
    <arg name="max_new_marker_error" default="0.05" />
    <arg name="max_track_error"      default="0.05" />
    <arg name="cam_image_topic"      default="/camera/depth_registered/points" />
    <arg name="cam_info_topic"       default="/camera/rgb/camera_info" />
    <arg name="output_frame"         default="/camera_rgb_optical_frame" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
</launch>

Change the marker size if needed and set the topics as per published and then launch the system in one go:
roslaunch alvar.launch


Checking the output:
Check the /ar_pose_marker topic for AR markers detected.
rostopic echo /ar_pose_marker

Use XBox controller with Turtlebot on ROS

Remote control (teleoperation) of the Turtlebot robot is very helpful. It is possible to use an XBox 360 controller for this purpose. This page will get you started. 

Software:
I used ROS Indigo.

Hardware:
Microsoft XBox 360 controller for Windows
(contains a Microsoft XBox 360 wireless receiver for Windows and a wireless controller)

Step 1: install hardware

• Connect the USB adapter and switch on the XBox controller
• Press the button on the adapter (light starts flashing)
• Press, and keep pressed the connect button on the back of the XBox controller : it looks like O)))
• Now the adapter and XBox controller are connected. Note that the lights on the XBox controller continue blinking.

to see all joystick devices connected run:
ls /dev/input/js* 

test which joystick it is connected to: in my case js0
sudo jstest /dev/input/js0

Make sure it is read/writeable for all:
sudo chmod a+rw /dev/input/js0


Step 2: basics on ROS
make sure ROS is running or start it via roscore

rosparam set joy_node/dev "/dev/input/js1"
rosrun joy joy_node

to see the response when you use the controller execute this statement in another terminal window
rostopic echo joy

Step 3: testing with turtlebot teleop
First bringup the turtlebot (on the turtlebot)
roslaunch turtlebot_bringup minimal.launch

Launch the joy node (on your workstation if you connected the adapter there)
rosparam set joy_node/dev "/dev/input/js0"
rosrun joy joy_node

Now start the teleop controls (on your workstation if you connected the adapter there)

rosparam set /joystick/dev "/dev/input/js0"
roslaunch turtlebot_teleop xbox360_teleop.launch

Important note: You need to press and hold the LB button on the XBox controller to use the joystick!

Note: I was expecting to need to install xboxdrv and joy but on my standard Indigo installation this was not needed.