Tf Data For Planned Path

edit

I'm aware that, when a planned path is executed by MoveIt, the coordinate transforms are published to the /tf topic. Let's say, however, that I only plan a trajectory and do not execute it. I still want the coordinate transformations of this planned path without executing it. It seems like there is no current topic for this. Thus, I went ahead and tried to create my own solution:

The wiki says,

robot_state_publisher uses the URDF specified by the parameter robot_description and the joint positions from the topic joint_states to calculate the forward kinematics of the robot and publish the results via tf.

I also know that the joint states of the planned path can be obtained from the move_group/display_planned_pathtopic (http://docs.ros.org/en/melodic/api/mo...)

So I created a node that subscribes to the move_group/display_planned_path topic and publishes the joint states of the planned path to a new topic /planned_joint_states. Then I created a new node of type robot_state_publisher and remapped it so that it subscribes to the /planned_joint_states topic and publishes tf data to a new /planned_tf topic.

After running all of this, I found that the joint states along the planned path were nicely published to the /planned_joint_states topic. However, nothing is ever published to the /planned_tf topic. I checked and everything is connected properly. In a separate window, I have run the demo_gazebo.launch file from the panda_moveit_config package so the robot_description parameter should be on the server. I'm not sure why this doesn't work but any help is much appreciated. My code is below for reference.

Launch File:

<launch>

   <node name="planned_joint_state_publisher" pkg="bag_ops" type="planned_joint_state_publisher" />

   <node name="planned_tf_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
       <remap from="joint_states" to="planned_joint_states" />
       <remap from="tf" to="planned_tf" />
   </node>

</launch>

Node:

 namespace bag_ops{

 class PlannedJointStatePublisher
 {
 protected:

     ros::NodeHandle nh;
     ros::Publisher pub;
     ros::Subscriber sub;

 public: 

     PlannedJointStatePublisher()
     {
         pub = nh.advertise<sensor_msgs::JointState>("/planned_joint_states", 1000);
         sub = nh.subscribe("/move_group/display_planned_path", 1000, 
 &PlannedJointStatePublisher::callback_topic_display_trajectory, this);

         ROS_INFO_STREAM("planned_joint_state_publisher node started...");
     }

     void callback_topic_display_trajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
     {
         robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
         for (size_t i = 0; i < msg->trajectory[0].joint_trajectory.points.size(); i++)
         {
             moveit::core::RobotState robot_state(robot_model_loader.getModel());
             moveit::core::jointTrajPointToRobotState(msg->trajectory[0].joint_trajectory, i, robot_state);
             sensor_msgs::JointState joint_state;
             moveit::core::robotStateToJointStateMsg(robot_state, joint_state);
             pub.publish(joint_state);
         }
     }

 };

 } // end namespace