Joint State Publisher

Building joint state publishing capabilities into ROS_Pub Class:

This allows for quick and easy publishing of a robot from anywhere in your C++ code where you can import ROS_Pub.

The API is as follows:

void new_js_channel(sejong::JS_Channel & jsc, std::string name, double minSleepTime=1000./30.);
void pub_js(sejong::JS_Channel & jsc, sejong::Vector & Q);

Here is some example code that shows how to do it:

// As usual, you have to declare the ROS_Pub pointer and a joint state channel:
ROS_Pub *rp;
rp = ROS_Pub::get_ROS_Pub();
sejong::JS_Channel jsc;

// new_js_channel() is just called once to set things up:
rp->new_js_channel(jsc, "ghost");

Note that the name of the js_channel must be the same as the tf_prefix specified in the launch file.

Later when you're ready to publish to a ghost in Rviz, you use the following:

// Q is a vector of joint states. It must be of size NUM_Q
rp->pub_js(jsc, Q);

You can pass in joint states and they are plotted in Rviz in real time:

The other part of the code that you would need to recreate this is in the launch file.  First you have to create a seperate tf_prefix for robot_state_publisher to read.  Note this is the same name as the "name" parameter given to the js_channel when it is created.  Then you have to remap from "joint_states" to "joint_states_"+name and from "tf_prefix" to tf_prefix_2".

In the launch file, it looks something like this:

In this case, it produces a ghosted spinning Valkyrie, with a second Valkyrie watching from afar: