How to subscribe trajectory_msgs/JointTrajectory and publish sensor_msgs/JointState

edit

Hi guys,

I am on Ubuntu 16.04 LTS and ROS Kinetic, working with MoveIt! and to move a real robotic arm named Robotis Manipulator-H. I am using the tutorial in this link in order to connect MoveIt! with the real robot: http://wiki.ros.org/Industrial/Tutori... . I created a node with an action server using the "join_trajectory_action" from this link: http://wiki.ros.org/industrial_robot_... that subscribes and publishes to the move_group node topics and now I want to convert the trajectory_msgs/JointTrajectory messages (that this node action server node provides) into sensor_msgs/JointStates messages. I want to do this because the node that is controlling my manipulator receives this kind of messages to set the goal states to the manipulator (and also publishes this same kind of messages to advertise its actual joint states). I want advice on how to do this, but I think that the correct way to do is by:

• Creating a node that subscribes trajectory_msgs/JointTrajectory messages and converts each waypoint one by one in to a sensor_msgs/JointState message and then send them to the node that controls the manipulator in order to make it move through the waypoints. This node that subscribes trajectory_msgs/JointTrajectory messages should also subscribe the topic where the actual joint states of the robot are published and create a control_msgs/FollowJointTrajectoryFeedback message to send movement/position feedback for the action server.

My questions are:

1. Is this the correct approach to solve this problem?
2. How do I know which point of the trajectory_msgs/JointTrajectory message should I convert into a sensor_msgs/JointState message to be sent to the node that controls my manipulator? Should I always use the first point given in the trajectory_msgs/JointTrajectory's trajectory_msgs/JointTrajectoryPoint[] array to be converted into a sensor_msgs/JointState message (and expect the trajectory_msgs/JointTrajectory message to have less points while the path is being executed)?
3. How do I fill the control_msgs/FollowJointTrajectoryFeedback message? I know I have to use information given from the robot for the actual position but my desired position varies once again with the trajectory_msgs/JointTrajectory message...

Thanks in advance!

Best regards, José Brito