accessing feedback from robot_command action

[TechGeniu]

Hey there,

I am trying to request feedback from RobotCommand while performing an ROSAction. Unfortunately, the feedback object is of type bosdyn_msgs/RobotCommandFeedback and the output is listed as below. Do you have a suggestion to access the se2_trajectory_feedback status from the feedback message? The output does not seem to be a Proto, therefore when I try to access the se2_trajectory_feedback, it throws the following error: AttributeError: 'RobotCommandFeedback' object has no attribute 'synchronized_feedback'

spot_msgs.action.RobotCommand_Feedback( feedback=bosdyn_msgs.msg.RobotCommandFeedback( command=bosdyn_msgs.msg.RobotCommandFeedbackOneOfCommand( full_body_feedback=bosdyn_msgs.msg.FullBodyCommandFeedback( feedback=bosdyn_msgs.msg.FullBodyCommandFeedbackOneOfFeedback( stop_feedback=bosdyn_msgs.msg.StopCommandFeedback(), freeze_feedback=bosdyn_msgs.msg.FreezeCommandFeedback(), selfright_feedback=bosdyn_msgs.msg.SelfRightCommandFeedback( status=bosdyn_msgs.msg.SelfRightCommandFeedbackStatus(value=0) ), safe_power_off_feedback=bosdyn_msgs.msg.SafePowerOffCommandFeedback( status=bosdyn_msgs.msg.SafePowerOffCommandFeedbackStatus(value=0) ), battery_change_pose_feedback=bosdyn_msgs.msg.BatteryChangePoseCommandFeedback( status=bosdyn_msgs.msg.BatteryChangePoseCommandFeedbackStatus(value=0) ), payload_estimation_feedback=bosdyn_msgs.msg.PayloadEstimationCommandFeedback( status=bosdyn_msgs.msg.PayloadEstimationCommandFeedbackStatus(value=0), progress=0.0, error=bosdyn_msgs.msg.PayloadEstimationCommandFeedbackError(value=0), estimated_payload=bosdyn_msgs.msg.Payload( guid='', name='', description='', label_prefix=[], is_authorized=False, is_enabled=False, is_noncompute_payload=False, version=bosdyn_msgs.msg.SoftwareVersion( major_version=0, minor_version=0, patch_level=0 ), version_is_set=False, body_tform_payload=geometry_msgs.msg.Pose( position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion( x=0.0, y=0.0, z=0.0, w=1.0 ) ), body_tform_payload_is_set=False, mount_tform_payload=geometry_msgs.msg.Pose( position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion( x=0.0, y=0.0, z=0.0, w=1.0 ) ), mount_tform_payload_is_set=False, mount_frame_name=bosdyn_msgs.msg.MountFrameName(value=0), mass_volume_properties=bosdyn_msgs.msg.PayloadMassVolumeProperties( total_mass=0.0, com_pos_rt_payload=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), com_pos_rt_payload_is_set=False, moi_tensor=bosdyn_msgs.msg.MomentOfIntertia( xx=0.0, yy=0.0, zz=0.0, xy=0.0, xz=0.0, yz=0.0 ), moi_tensor_is_set=False, bounding_box=[], joint_limits=[] ), mass_volume_properties_is_set=False, preset_configurations=[] ), estimated_payload_is_set=False ), constrained_manipulation_feedback=bosdyn_msgs.msg.ConstrainedManipulationCommandFeedback( status=bosdyn_msgs.msg.ConstrainedManipulationCommandFeedbackStatus(value=0), desired_wrench_odom_frame=geometry_msgs.msg.Wrench( force=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), torque=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0) ), desired_wrench_odom_frame_is_set=False ), feedback_choice=0 ), status=bosdyn_msgs.msg.RobotCommandFeedbackStatusStatus(value=0) ), synchronized_feedback=bosdyn_msgs.msg.SynchronizedCommandFeedback( arm_command_feedback=bosdyn_msgs.msg.ArmCommandFeedback( feedback=bosdyn_msgs.msg.ArmCommandFeedbackOneOfFeedback( arm_cartesian_feedback=bosdyn_msgs.msg.ArmCartesianCommandFeedback( status=bosdyn_msgs.msg.ArmCartesianCommandFeedbackStatus(value=0), measured_pos_tracking_error=0.0, measured_rot_tracking_error=0.0, measured_pos_distance_to_goal=0.0, measured_rot_distance_to_goal=0.0 ), arm_joint_move_feedback=bosdyn_msgs.msg.ArmJointMoveCommandFeedback( status=bosdyn_msgs.msg.ArmJointMoveCommandFeedbackStatus(value=0), planner_status=bosdyn_msgs.msg.ArmJointMoveCommandFeedbackPlannerStatus(value=0), planned_points=[], time_to_goal=builtin_interfaces.msg.Duration(sec=0, nanosec=0), time_to_goal_is_set=False ), named_arm_position_feedback=bosdyn_msgs.msg.NamedArmPositionsCommandFeedback( status=bosdyn_msgs.msg.NamedArmPositionsCommandFeedbackStatus(value=0) ), arm_velocity_feedback=bosdyn_msgs.msg.ArmVelocityCommandFeedback(), arm_gaze_feedback=bosdyn_msgs.msg.GazeCommandFeedback( status=bosdyn_msgs.msg.GazeCommandFeedbackStatus(value=0), gazing_at_target=False, gaze_to_target_rotation_measured=0.0, hand_position_at_goal=False, hand_distance_to_goal_measured=0.0, hand_roll_at_goal=False, hand_roll_to_target_roll_measured=0.0 ), arm_stop_feedback=bosdyn_msgs.msg.ArmStopCommandFeedback(), arm_drag_feedback=bosdyn_msgs.msg.ArmDragCommandFeedback( status=bosdyn_msgs.msg.ArmDragCommandFeedbackStatus(value=0) ), arm_impedance_feedback=bosdyn_msgs.msg.ArmImpedanceCommandFeedback(), feedback_choice=0 ), status=bosdyn_msgs.msg.RobotCommandFeedbackStatusStatus(value=0) ), arm_command_feedback_is_set=False, mobility_command_feedback=bosdyn_msgs.msg.MobilityCommandFeedback( feedback=bosdyn_msgs.msg.MobilityCommandFeedbackOneOfFeedback( se2_trajectory_feedback=bosdyn_msgs.msg.SE2TrajectoryCommandFeedback( status=bosdyn_msgs.msg.SE2TrajectoryCommandFeedbackStatus(value=2), body_movement_status=bosdyn_msgs.msg.SE2TrajectoryCommandFeedbackBodyMovementStatus(value=1) ), se2_velocity_feedback=bosdyn_msgs.msg.SE2VelocityCommandFeedback(), sit_feedback=bosdyn_msgs.msg.SitCommandFeedback( status=bosdyn_msgs.msg.SitCommandFeedbackStatus(value=0) ), stand_feedback=bosdyn_msgs.msg.StandCommandFeedback( status=bosdyn_msgs.msg.StandCommandFeedbackStatus(value=0), standing_state=bosdyn_msgs.msg.StandCommandFeedbackStandingState(value=0) ), stance_feedback=bosdyn_msgs.msg.StanceCommandFeedback( status=bosdyn_msgs.msg.StanceCommandFeedbackStatus(value=0) ), stop_feedback=bosdyn_msgs.msg.StopCommandFeedback(), follow_arm_feedback=bosdyn_msgs.msg.FollowArmCommandFeedback(), feedback_choice=1 ), status=bosdyn_msgs.msg.RobotCommandFeedbackStatusStatus(value=1) ), mobility_command_feedback_is_set=True, gripper_command_feedback=bosdyn_msgs.msg.GripperCommandFeedback( command=bosdyn_msgs.msg.GripperCommandFeedbackOneOfCommand( claw_gripper_feedback=bosdyn_msgs.msg.ClawGripperCommandFeedback( status=bosdyn_msgs.msg.ClawGripperCommandFeedbackStatus(value=0) ), command_choice=0 ), status=bosdyn_msgs.msg.RobotCommandFeedbackStatusStatus(value=0) ), gripper_command_feedback_is_set=False ), command_choice=2 ) ) )

[bhung-bdai]

Hi @TechGeniu,

The proto message here states that you can only have one active type of command on the robot. Depending on what kind of command you are issuing to the ROS2 Action server, you may not be able to access the synchronized_feedback. If you have an example of the command in a clearer format, that would be helpful as well.

[bhung-bdai]

Apologies for the confusion. I've discovered that RobotCommand_Feedback has a .feedback field. So you would need to do

RobotCommand_Feedback.feedback.<rest of your command>

in order to get the synchronized command feedback. I'm not sure why this is the case, will follow up with the proto generators.

[TechGeniu]

Hey @bhung-bdai,

Thank you for your prompt response. Initially, I was not aware that RobotCommand_Feedback requires an additional .feedback field. However, I later found the usage of this feedback in few SDK examples. Although, I tried to access the synchronized command feedback like you had suggested, I still get the Attribute error as I mentioned. I was testing this on the walk_forward example. I have made the following changes to the walk_forward.py

future = self._robot_command_client.send_goal_and_wait(
            action_goal, self.feedback_callback)

        self.get_logger().info("Successfully walked forward")

def feedback_callback(self, feedback_msg):
        response = feedback_msg.feedback

        self.get_logger().info(f"======================================")
        self.get_logger().info("Trajectory Status: {0}".format(response))

        trajectory_status = (
            response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status
        )

        self.get_logger().info(f"======================================")
        self.get_logger().info("Trajectory Status {0}".format(trajectory_status))

I have also made the relevant change in the ActionClientWrapper to pass the feedback_callback as a parameter to the send_goal_and_wait function.

Additionally, I was trying to access the result field as well. I was able to get Success and Message fields, however, I was not able to get an output for a synchronized feedback in result as well. I am not sure, if I am accessing the protos wrong. It would be great if you can suggest where I am going wrong, in accessing the protos.

[bhung-bdai]

Ah, I understand the issue.

The design of send_goal_and_wait is a synchronous call to the action server. It will send a goal, get a future, and wait until that future responds. It is not meant to be queried during the execution for feedback as it should be blocking until the action server returns a result status.

Ideally, you would be using the send_goal_handle function along with the ActionHandle object provided in bdai_ros2_wrappers, which is meant to call a function asynchronously. This would result in something like:

action_handle = ActionHandle(action_name)
action_handle.set_feedback_callback(feedback_callback)
send_goal_future = action_client.send_goal_async(goal, feedback_callback=action_handle.get_feedback_callback)
action_handle.set_send_goal_future(send_goal_future)

This creates an ActionHandle object and sets a feedback callback which is called periodically by the action_client with the feedback from the ActionServer. It then sets the result callback to be called once the goal's future is returned.

I apologize for the confusion. Synchronous programming in ROS2 is generally best avoided, so we build in workarounds here. In theory your way should work as well, but that wasn't how it was intended.

[bhung-bdai]

The other issue is that I think the ROS2 messages are incorrectly accessed. If I were to get an se2_trajectory_feedback, it would look more like this: feedback.feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status

If you need a reference, you can search the bosdyn_msgs repo to understand the structure. These ROS2 messages are 1-to-1 mappings of the Boston Dynamics Spot-SDK protobuffers as far as I know, so they are quite verbose. You should be able to track down the current keywords by finding the right field and code searching it in the repo.

[TechGeniu]

Hey @bhung-bdai ,

Thank you for your prompt response. I did not notice the latest changes made to the Spot_wrapper and I was using an older version. I was able to access the se2_trajectory_feedback, like you had suggested.

Thanks for the help!

[bhung-bdai]

Closing unless otherwise notified.