Robot not responding to goal commands

[matthew-rt]

Hello,

I'm having an issue where commands which send the robot to a particular goal get no response.

My settings:
ROS Noetic
Spot SDK Version 3.2
I followed the steps detailed in the instructions and installed the spot wrapper through the steps detailed in #120

I can launch spot_driver with no errors, and claim the lease and make the robot stand with no issues.

I can publish commands to the /spot/cmd_vel topic, which move the robot with no issues, and I can use the actionlib server on /spot/body_pose to change the pose of the robot when stationary.

However, when I attempt to give Spot goals, using either the 2d nav goal in rviz, the /spot/go_to_goal topic, and the actionlib server at /spot/trajectory , the command has no effect.

If I give the goals through any of the above methods, the spot_driver terminal window reads
[INFO] [1682598013.010615]: got command duration of 5.0

but the robot does not move. The actionlib_tools client returns a success.

I have also tried controlling the movement through a python script using the TrajectoryActionGoal message. I'll omit the full code where the message is populated for brevity, but the client is set up with:
self.actionclient=actionlib.SimpleActionClient('/spot/trajectory',TrajectoryAction)

and the message is sent with:
self.actionclient.send_goal(goalmessage)

which returns the error:
AttributeError: 'TrajectoryActionGoal' object has no attribute 'target_pose'

However, before sending goalmessage I verified that it has the attribute target_pose by printing it, which returns that section of the message with no errors.

I can provide the full error message or the full code for populating the message if that's useful: I also am not sure if this is two separate errors, or if they are connected somehow.

[heuristicus]

A possible cause can be that you have the autonomy_enabled param set to false, but this would show up as an error message if the command was forbidden as a result of that.

I think you are likely sending an incorrect goal - it should be TrajectoryGoal and not TrajectoryActionGoal.

A useful thing to know would be whether you have the same issue if you use the code from the 1.0.0 tag, but try this after checking the other stuff. I tested the new wrapper implementation and didn't notice any issues with trajectory navigation.

[matthew-rt]

You're correct, I changed TrajectoryActionGoal to TrajectoryGoal, and the code stopped returning an error, but the robot still didn't move after the command was sent.

I checked autonomy_enabled, which was set to True, and then tried using the code under the 1.0.0 tag, and the problem still persisted.

I then checked the driver launch file, and noticed the lines which set the maximum x and y linear speed, with the comment stating that leaving these speeds at 0 applies spot's internal limits.

On a whim, I changed these arguments to 1.5 and 1.5, and now the robot responds to trajectory commands!

I'm not sure if my robot's speed limits are set internally on some control panel or something, but its working now.

To fix this, such that a warning is added, I tried adding this to the robot_allowed_to_move function, on line 643 of spot_ros:

   velocity_ok=True

  mobility_params = self.spot_wrapper.get_mobility_params()
  max_lin_vel_x=mobility_params.vel_limit.max_vel.linear.x
  max_lin_vel_y=mobility_params.vel_limit.max_vel.linear.y

  if max_lin_vel_x==0 or max_lin_vel_y==0:
      rospy.logwarn("Spot will not move as its linear x or y velocity is set to 0. ")
      velocity_ok=False

  return self.allow_motion and autonomy_ok and velocity_ok

This works fine, and gives a warning, but it has the unintended side effect of stopping standing if the x and y velocity limits are set to 0, which isn't desired.

Instead I've added the following to the handle_trajectory function:

    mobility_params = self.spot_wrapper.get_mobility_params()
    max_lin_vel_x=mobility_params.vel_limit.max_vel.linear.x
    max_lin_vel_y=mobility_params.vel_limit.max_vel.linear.y

    if max_lin_vel_x==0 or max_lin_vel_y==0:
        rospy.logerr(
            "Spot will not move as its linear x or y velocity is set to 0. "
            )
        self.trajectory_server.set_aborted(
            TrajectoryResult(False, "Velocity limits set to 0.")
        )
        return

This seems like a good solution to me: let me know if you think there's a better way of handling it. If not, I'll commit these changes and push them to the repo

[heuristicus]

Interesting, thank you for working out the cause of this. I think your addition to the handle trajectory function is sensible and likely the best solution. I don't recall exactly when I added the velocity limits, but there have been several updates to the SDK since then so it's possible that something has changed about how it works in terms of velocity limits. Since the default seems to no longer function, it would be a good idea to also change the defaults to some relatively low values rather than zero.

Your check may also need to look at the angular velocity, since I assume if that is set to zero that the robot cannot rotate, which will limit its movement as well.