I guess I found why it’s not working:
The Joint Trajectory Controller ‘/niryo_one_follow_joint_trajectory_controller/command’ topic is used to send direct command to the hardware. If you use this interface you’d have to compute or interpolate each points of the trajectory before sending them to the ros_control controller.
You can think of this ‘/niryo_one_follow_joint_trajectory_controller/command’ interface as an interface most suited for ‘jog’ commands. And that’s exactly what we use for moving the robot with a Joystick controller (see code on github)
What you need here is not the ‘/niryo_one_follow_joint_trajectory_controller/command’ topic, but the ‘/niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory’ action. (see more about this controller here)
How things are processed with Niryo One (quick recap):
– The RobotCommander will act as a global gateway, to ensure only one command is processed at the same time
– It contains a MoveGroup interface to plan and send trajectories
– The trajectory is sent directly from Moveit with its simple_controller_manager functionality
– ros_control receives the trajectory, interpolates between points, and send commands to hardware.
For your application, you can rewrite the first part (as you did here), keep the MoveGroup interface to only compute trajectories. Then send the trajectory yourself instead of using the Moveit functionality (self.arm.execute() or self.arm.move()), and finally, nothing to do on the ros_control side.
– In your RobotCommander class, create an ActionClient, or the simpler-to-use SimpleActionClient (the simple action client can only handle one goal from the client side, but for your application that’s just what you need).
– Keep your callback_trajectory_command function with the ‘/niryo_one/commander/trajectory’ subscriber.
– In the callback, send a new ActionGoal through your SimpleActionServer (and not through the topic interface)
And that should be it! Everytime your subscriber receives a new command, it will compute a plan and send it to the controller. The controller will then use its trajectory replacement feature.
Let me know if it doesn’t make sense :)