Hi,
Is there any way to control the head separately from the rest of the body? I tried setting up a separate node that controls the head movements so that it follows a person, but when I move the body as well using the joint trajectories, they seem to interrupt each other. I am using ROS1 noetic. Do you know if there is any way to get around this, such as creating a new namespace for a separate stretch controller action server only for the head? Thank you!
Hi @moore.dr, Could you provide us with more info or post the code to reproduce the issue you are facing?
As per the stretch_ros design, you are expected to have only one instance of the Stretch Driver node running, which will start the Joint Trajectories action server. You can create multiple Joint Trajectory Action client nodes to command different sets of robot joints.
Hi @Mohamed_Fazil, thank you for getting back to me. The issue I am currently facing is when I am sending multiple joint trajectory goals through Hello Node’s move_to_pose function. I have two different nodes repeatedly calling this function simultaneously, one to control the head and the other to control the arm/wrist, which results in one of the actions terminating prematurely. I was wondering if it was possible to achieve both head and arm movement through this setup, or if there is alternative approach I should be using. Thank you!
The driver node is treating the subsequent commands as “preempting” or replacing the previous commands because it isn’t looking at the specific joints in each command when performing the preemption.
The easiest solution would be to unify the head publisher with the arm/wrist publisher, so as to send commands that include both sets of joints. But if your application requires the two nodes to be separate, you might try using a TimeSynchronizer from the message_filters package to sync up the two sets of commands in a third node, before forwarding it on to the driver node.
Thank you for the suggestion! Another issue with my current setup is that my head command publisher sends commands far more often in order to track a person. In this situation, the time synchronizer probably wouldn’t work as even the synced commands would be overwritten by a new head command almost immediately. Do you have any recommendations for how to approach this?
That’s true, the time synchronizer would have to rate limit the head commands in order to match the rate of the arm/wrist commands. Your arm/wrist publisher could subscribe to the head commands to incorporate the head joints into the arm command, so the arm/wrist publisher would publish whole body commands at a lower rate without preempting the head publisher. But this approach can cause stuttering or backwards motion if the network conditions were poor, and the arm/wrist node isn’t receiving the latest head commands fast enough. I would attempt this if all 3 nodes were running locally.
A more involved option would be to edit the Stretch Driver node directly to separate the joint trajectory action server into two separate action servers, one for the head and one for the rest of the robot. The stretch driver node instantiates and starts the action server here:
And the joint trajectory action server is defined in this file:
Separating the action server into two, would involve copying the code into a second action server, and modifying the “command groups” (i.e. the joints supported by the action server):
I’ve provided links to the ROS 1 Noetic codebase because you’d mentioned that what you are using, but our ROS 1 repo is currently in maintenance mode (i.e. we’re fixing bugs, but not adding new functionality), and is set to be deprecated on Jan 2025. If you’re interested in making these code changes and contributing back the functionality, I’d recommend making the switch to Stretch’s ROS 2 Humble software (there’s an upgrade guide available here) because we’re going to be supporting ROS 2 going forward.