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.