Sending movement / Waypoint Commands through topics/services

[arihantpandey]

Hi,

What is the syntax for sending local waypoint commands through available ros2 interfaces?
I have tried sending

ros2 topic pub /body_pose geometry_msgs/Pose "position:
  x: 1.0
  y: 0.0
  z: 0.0"

and the robot does not respond at all.

I have also tried sending

ros2 topic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.3" -r 10

and while the robot does seem to respond to the command, the movement can be best described as "hesitant" and jerky. I suspect I am doing something wrong!

On a similar note, I noticed an available service called robot_command, is there documentation on how I can use this service to construct more complex commands? Would commands sent through this interface block until completed, or only block until the request is received?

[khughes-bdai]

Hi!
For your first question -- in the callback for body_pose, it looks like x and y arguments of the topic are discarded, and only the z is used. I think this is because you can adjust the height that the robot is standing at, but it doesn't make sense to translate the body laterally while keeping the feet on the ground. If you publish to the orientation part of this message, the robot should twist its body to try to achieve this pose while remaining standing.

spot_ros2/spot_driver/spot_driver/spot_ros2.py

Line 2508 in c2123e9

def body_pose_callback(self, data: Pose) -> None:

For your second question, the timing for the cmd_vel topic is important in order to ensure a smooth trajectory. Under the hood these commands are being sent to the robot with a "time to live" that is specified by the cmd_duration parameter. You can try either increasing this param, or try sending cmd_vel at a faster rate to make sure that each robot command isn't completing before another one is sent. (another discussion about this here that might be helpful: #394)

Lastly, the driver provides robot_command services and actions. These are meant to be a thin wrapper on BD's robot_command API, which is a very large message containing a lot of fields. Since it is easier to construct the proto rather than the ROS message for robot command (less fields to fill out), we recommend creating more complex commands with a tool like BD's robot command builder, then convert it to a ROS message that can be used in the service or action request using the convert function from our bosdyn_msgs package. We are working on our documentation for this repo and will make sure to add this info in.

from bosdyn_msgs.conversions import convert
from bosdyn_msgs.msg import RobotCommand
...
proto_robot_command = ... # generate this using BD's protos
ros_robot_command = RobotCommand()
convert(proto_robot_command, ros_robot_command)
# then you can use this to fill in the service or action request , i.e.
service_request = RobotCommandService

Using these robot commands shouldn't block. For the action specifically, you will receive feedback on the status of the command's execution and in your code can choose how to handle that.

[arihantpandey]

Hi,

Thank you for the response. What would be the best course of action to command the robot to move forwards one meter using the available topics/services? Is there a way to send a ros message directly with a goal position to the robot without using BD's robot command builder directly or doing what you did in the walk_forward.py example?

In terms of the driver's robot_command service, is there any documentation of the format that the request has to be in order to be able to send it to the robot_command ros2 service? I see in the handle_robot_command_service() function that is expected to be of the form of RobotCommandService.Request, but I cannot find any examples of what this looks like.

Thanks again for your help.

[khughes-bdai]

Unfortunately, there is currently not a super streamlined way to send a command to walk 1m forward through the available topics and services. I would say the easiest course of action at the moment is to use BD's command builder in a node similarly to walk_forward.py. The current structure of the RobotCommand message requires that you specify where you are walking to as a pose in the global coordinate frame so you would have to do some frame lookups and transformation math in order to accomplish this. The other option could be to add a service/action to the driver to accomplish this specific conversion internally.

The robot command service is of type spot_msgs/srv/RobotCommand

spot_ros2/spot_driver/spot_driver/spot_ros2.py

Line 138 in 668a3b7

from spot_msgs.srv import ( # type: ignore

RobotCommandService.Request contains a command (bosdyn_api_msgs/RobotCommand) and a duration https://github.com/bdaiinstitute/spot_ros2/blob/main/spot_msgs/srv/RobotCommand.srv.
The bosdyn_api_msgs package auto generates these ros messages from BD's protobufs, you can look at what fields are in here with ros2 interface show bosdyn_api_msgs/RobotCommand. It should match what is in BD's robot command proto (here)

[khughes-bdai]

You can also try looking into the /trajectory action for walking the robot forward. The interface is a lot simpler than RobotCommand but you still will probably have to do a frame look up in order to fill in the desired end pose https://github.com/bdaiinstitute/spot_ros2/blob/main/spot_msgs/action/Trajectory.action

[khughes-bdai]

Hi, All of our actions should be set up as interruptible -- it should try to cancel the previous command, then send the most recent one. (I know this is the case with the robot command action and it *should* be the case with trajectory, but I personally haven't worked with trajectory before).

…

On Mon, Sep 23, 2024 at 9:35 AM Arihant Pandey ***@***.***> wrote: Hi, thank you for pointing out the trajectory action. Is this action interruptible i.e if I command a trajectory and then want to override it and cancel/reroute the path is this possible? — Reply to this email directly, view it on GitHub <#483 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/BFRBBBXLE6ZLMDQDKZ7E35DZYAKLDAVCNFSM6AAAAABOOOYRFWVHI2DSMVQWIX3LMV43URDJONRXK43TNFXW4Q3PNVWWK3TUHMYTANZSGY3DGMY> . You are receiving this because you commented.Message ID: ***@***.*** com>

[arihantpandey]

Amazing, thanks.

I want to be able to create an action client from outside of the repo. Is there a way to compile the spot custom messages on its own in ubuntu 20.04?

[khughes-bdai]

This repo is designed to work with ROS 2 Humble on 22.04. If you have a 20.04 computer I'd suggest using the provided Dockerfile to ensure you have the correct environment. This will use the pre-built binary bundles from the bosdyn_msgs package which is installed here. You can then use these in any other location you choose inside the docker container.
You can always choose to compile bosdyn_msgs from scratch, it will just take a long time since there are a ton of messages. There's instructions here https://github.com/bdaiinstitute/bosdyn_msgs

[arihantpandey]

Thank you for all your help so far.

I have been doing some testing using the Trajectory Action server. I have noticed that very often when commanding a waypoint (e.g 1m in x direction) by using the async_send_goal method in my action client, the robot will correctly move but will not correctly update its status and simply stop moving while continually sending "Moving to goal" as feedback. I was wondering if this is a known issue or if there is likely something wrong with my implementation.

Another more general question is that is there any particular reason why the Trajectory action server only accepts the body frame?

Thanks again,
Arihant

[khughes-bdai]

Hi Arihant,
Apologies for the delayed response! If you want to post the relevant part of your code in here I'd be happy to take a look at it. It is possible that there are some bugs in its implementation, I have not used it before personally and I don't think it's been updated in a while. There also doesn't appear to be a specific reason why only the body frame was supported -- it is sort of hard coded into spot_wrapper in the underlying function called in the action, but hypothetically could be expanded to support arbitrary frames.