Visualization of Orientations in Rviz

I finally completed the orientation visualization for Rviz.  The idea is to be able to visualize an X, Y, and Z unit vector in the local frame of a particular body.  Orientations are definitely trickier than I thought they would be.  Here's a quick overview of my method:

To obtain the local frame's X, Y, and Z unit vectors I am using:

// with i = 0, 1, and 2 for X, Y, and Z
quaternion._transformVector( sejong::Vect3::Unit(i) );

This syntax is the same for position markers, and the same static ROS_Pub class handles both position and orientation publishing:

// Get a static instance of the ROS_Pub class:
rp_ = ROS_Pub::get_ROS_Pub();

Next, you ask ROS_Pub for orientation marker channels.  It initializes the channels to your specified color.  Notice that since, we typically do not want orientation trails, we set the maxNumMrks = 1, which means that each new update will replace the previous orientation marker.  Also, in this case, the channel will update at the most once every 20 ms.  This is to prevent the publishing from overloading your processor and stealing resources from the controller.

rp_->get_new_ori_marker("act_hand_ori", BLUE, mc3_, 1, 20.);
rp_->get_new_ori_marker("des_hand_ori", LIME, mc4_, 1, 20.);

Now that the client has an orientation marker channel, in order to publish an orientation, all you need is a quaternion.  Pass the quaternion to the pub_ori_marker channel along with a root position where you want the axes' origin to be located and it will plot a Red, Green, and Blue arrow on the local X, Y, and Z axes designating the body local orientation specified by the passed quaternion.  The color specified in the get_new_ori_marker() method will identify the three axes in that channel with a colored sphere on the tip of the axis arrows.

rp_->pub_ori_marker(mc3_,pos, act_quaternion );
rp_->pub_ori_marker(mc4_,pos, des_quaternion );

The following video shows some example output.  Axes with green dots display the desired orientation (which is a sinusoidal oscillation about global x-axis), and the axes with blue dots display the actual orientation.  The controller is not implemented which is why the hand is not following the orientation motion plan.