Handle cases when mobility command feedback does not contain a specific command status

[Imaniac230]

Hi,

I have stumbled upon a bit obscure error message from the ROS2 driver when attempting to send sit commands to the robot while it was also actively receiving velocity movement commands. This might also be related to:

• Restructure AsyncIdle #47
• Sit/stand status is not updated after publishing to /cmd_vel spot_ros2#260

It makes sense that the sit commands will always be overridden by the movement commands and therefore the robot will refuse to sit down. However, the ROS2 driver responds to this by a outputting an error message, ex.: [spot_ros2-5] [wrapper.py:236] Error when getting robot command feedback: bosdyn.api.RobotCommandFeedbackResponse (InternalServerError): Command not found, id: 62532, which was quite confusing.

I have found the root cause of the error in this parsing logic:

spot_wrapper/spot_wrapper/wrapper.py

Lines 223 to 237 in 99ec4a6

	if self._spot_wrapper.last_sit_command is not None:
	try:
	self._spot_wrapper.is_standing = False
	response = self._client.robot_command_feedback(self._spot_wrapper.last_sit_command)
	if (
	response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status
	== basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING
	):
	self._spot_wrapper.is_sitting = True
	self._spot_wrapper.last_sit_command = None
	else:
	self._spot_wrapper.is_sitting = False
	except (ResponseError, RpcError) as e:
	self._logger.error("Error when getting robot command feedback: %s", e)
	self._spot_wrapper.last_sit_command = None

, specifically this status parsing:

spot_wrapper/spot_wrapper/wrapper.py

Lines 227 to 230 in 99ec4a6

	if (
	response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status
	== basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING
	):

The normal feedback response looks like this:

feedback {
  synchronized_feedback {
    mobility_command_feedback {
      sit_feedback {
        status: STATUS_IS_SITTING
      }
      status: STATUS_PROCESSING
    }
  }
}

However, when the command is overridden, the feedback response looks like this:

feedback {
  synchronized_feedback {
    mobility_command_feedback {
      status: STATUS_COMMAND_OVERRIDDEN
    }
  }
}

and this is where the the logic throws an exception because the sit_feedback section does not exist in the message at all. The same also applies to the stand command. I think the same might also apply to the trajectory command, but I have not tested that one.

I have added a top-level check for the value of mobility_command_feedback.status before any further parsing is performed. If this status is not currently STATUS_PROCESSING then the command is no longer valid and its last_*_command variable is reset together with a warning message.

This does not modify the actual robot behavior in any way, but avoids the obscure error message and instead handles the cases explicitly. I'm not sure if there are any larger refactors for the AsyncIdle class being currently in the works relating to #47 and so if this small proposal has any value currently, but I am putting it out here.

I've also added a final check at the end, which might relate to the issue here bdaiinstitute/spot_ros2#260. If the robot is in a moving state, the sitting and standing states will be set accordingly. This would make sense since the movement commands currently always force the robot to stand up a will override any sit and stand results. But, this may not be an optimal solution if the behavior changes in the future.

[mhidalgo-bdai]

Fix looks appropriate though a regression test would be nice.

[Imaniac230]

I see that the linter tests are failing. I'll get it fixed as soon as I get back to it. Sorry for that, I should have ran the linter checks beforehand.

[khughes-bdai]

Looks good to me once the linting is fixed. I just tested on robot and verified that it does resolve bdaiinstitute/spot_ros2#260. Thanks!

[Imaniac230]

Looks like the uploading to Coveralls stage is failing with a missing repo token:

Missing Repo Token. Set using `-r <token>` or `COVERALLS_REPO_TOKEN=<token>`

I'm not sure if there is anything I can try to fix that without actually triggering multiple CI runs.

[mhidalgo-bdai]

@amessing-bdai any clue what may be going on here? Was the token removed? Did it expire?

[amessing-bdai]

@amessing-bdai any clue what may be going on here? Was the token removed? Did it expire?

No, it wasn't removed and shouldn't have expired (there isn't a timeout on it that I can see).

That step is only supposed to run on 3.10, but for some reason is running on the whole matrix.

[khughes-bdai]

I think that the token issue is because this PR is from outside of the bdai organization and doesn't have the same permissions. I have seen it happen before on other external PRs. I am not sure why the tests are happening on 3 different versions of Python, but the actual tests are passing, it is just the coveralls step that is failing