Visualization of Trajectories in Rviz

Added a singleton class that automatically handles publishing position markers in Gazebo. 

Example syntax is as follows for the end user:

// Declare ROS_Publisher variable with some sejong MarkerChannels in your .h file:
ROS_Pub* rp_;
sejong::MarkerChannel mc1_;
sejong::MarkerChannel mc2_;

Then you get the static instance of the publisher in the constructor of your controller and create some markers:

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

// Call this method to get a marker for a specific color or purpose:
rp_->get_new_pos_marker("actual", BLUE, mc1_, 500, 10.);
rp_->get_new_pos_marker("desired", LIME, mc2_, 500, 10.);

You can pass in a channel name, the channel's marker color, the number of markers in this channel that will remain visible, and the max update rate.

Whenever you want to publish a position, all you have to do is call the pub_pos_marker() method which takes the marker_channel and a 3D vector with your position.  It handles all the publishing in the background.  It is much cleaner than try to publish manually.

rp_->pub_pos_marker(mc1_,act);
rp_->pub_pos_marker(mc2_,des);

In the following screenshot, Valkyrie is using WBOSC for a hand position task.

You can see the tracking is fairly good between Desired (GREEN) and Actual (BLUE).