Tf Data For Planned Path
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 parameterrobot_description
and the joint positions from the topicjoint_states
to calculate the forward kinematics of the robot and publish the results viatf
.
I also know that the joint states of the planned path can be obtained from the move_group/display_planned_path
topic (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
I haven't tried doing this so I'm not sure what the issue is, but is there a reason you wouldn't use the TF broadcaster?