From 2f58b5a744ad174d3a48bd2cadb984a4b3551f82 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Wed, 13 Sep 2023 10:51:00 -0600 Subject: [PATCH 01/19] added beginning of test --- spot_driver/test/test_spot_driver.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index e716920dc..2df433279 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -3,9 +3,11 @@ import bdai_ros2_wrappers.scope as ros_scope import rclpy +from bosdyn_msgs.msg import RobotCommandFeedback from std_srvs.srv import Trigger import spot_driver.spot_ros2 +from spot_driver.spot_ros2 import GoalResponse from spot_msgs.srv import ( # type: ignore Dock, ) @@ -65,6 +67,17 @@ def test_wrapped_commands(self) -> None: resp = self.dock_client.call(Dock.Request()) self.assertEqual(resp.success, True) + def test_robot_command_goal_complete(self) -> None: + self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) + + feedback = RobotCommandFeedback() + + # Testing FullBodyFeedback + feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET + + feedback.command.full_body_feedback.status.value = feedback.command.full_body_feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + if __name__ == "__main__": unittest.main() From c79e43c61f6911c3febe0cdb24bddf805c5c4686 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 19 Oct 2023 14:04:26 -0600 Subject: [PATCH 02/19] Added test cases for method --- spot_driver/test/test_spot_driver.py | 556 ++++++++++++++++++++++++++- 1 file changed, 554 insertions(+), 2 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 2df433279..8922274c0 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -67,15 +67,567 @@ def test_wrapped_commands(self) -> None: resp = self.dock_client.call(Dock.Request()) self.assertEqual(resp.success, True) + # Ignore Line too long errors + # ruff: noqa: E501 def test_robot_command_goal_complete(self) -> None: + FEEDBACK_INVALID = -128 + self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) feedback = RobotCommandFeedback() - # Testing FullBodyFeedback + """ Testing FullBodyFeedback """ + + fullbody_feedback = feedback.command.full_body_feedback feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET - feedback.command.full_body_feedback.status.value = feedback.command.full_body_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_PROCESSING + + """ Testing STOP_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing FREEZE_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing SELFRIGHT_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_COMPLETED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing SAFE_POWER_OFF_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing BATTERY_CHANGE_POSE_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + """ Testing PAYLOAD_ESTIMATION_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing CONSTRAINED_MANIPULATION_FEEDBACK_SET """ + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + """ Testing Synchronized Feedback Command """ + arm_command_feedback = feedback.command.synchronized_feedback.arm_command_feedback + feedback.command.command_choice = feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET + + """ Testing arm command feedback """ + feedback.command.synchronized_feedback.arm_command_feedback_is_set = True + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False + + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_COMMAND_OVERRIDDEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_PROCESSING + ) + + """ Testing arm cartesian feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + # should these next two test cases be failures? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing arm joint move feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing named arm position feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing arm velocity feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET + ) + # Arm velocity commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing arm gaze feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing arm stop feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET + ) + # Arm stop commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing arm drag feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + """ Testing arm drag feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + ) + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing arm drag feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + ) + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing unknown arm command """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing mobility commands """ + mobility_feedback = RobotCommandFeedback().command.synchronized_feedback.mobility_command_feedback + + feedback.command.synchronized_feedback.arm_command_feedback_is_set = False + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = True + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False + + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_COMMAND_OVERRIDDEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_PROCESSING + ) + + """ Testing se2 trajectory feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing se2 velocity feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET + ) + # Planar velocity commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing sit feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_IS_SITTING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing stand feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_IS_STANDING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing stance feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET + ) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_STANCED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing stop feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + ) + # Stop commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing stop feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET + ) + # follow arm commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing stop feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_NOT_SET + ) + # mobility command feedback is not set, this could be caused by a command that finishes and resets the feedback status. + # because of this case, it will return success as long as no other synchronous commands are run afterwards. + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing unknown command """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing Gripper commands """ + gripper_feedback = RobotCommandFeedback().command.synchronized_feedback.gripper_command_feedback + + feedback.command.synchronized_feedback.arm_command_feedback_is_set = False + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = True + + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_COMMAND_OVERRIDDEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_PROCESSING + ) + + """ Testing Claw Gripper feedback """ + feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = ( + gripper_feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET + ) + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + + """ Testing unknown gripper command """ + feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = FEEDBACK_INVALID + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + """ Testing unknown robot command type """ + feedback.command.command_choice = FEEDBACK_INVALID self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) From 0f9913a6ad1b9168a89f351a55a145d2f85f3554 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 19 Oct 2023 17:27:56 -0600 Subject: [PATCH 03/19] refactor _robot_command_goal_complete --- spot_driver/spot_driver/spot_ros2.py | 402 ++++++++++++++------------- 1 file changed, 203 insertions(+), 199 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index b75c7dd3f..a792ffd97 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -39,8 +39,12 @@ from bosdyn.client import math_helpers from bosdyn.client.exceptions import InternalServerError from bosdyn_msgs.msg import ( + ArmCommandFeedback, + FullBodyCommandFeedback, + GripperCommandFeedback, ManipulationApiFeedbackResponse, ManipulatorState, + MobilityCommandFeedback, RobotCommandFeedback, ) from geometry_msgs.msg import ( @@ -1466,248 +1470,248 @@ def handle_max_vel(self, request: SetVelocity.Request, response: SetVelocity.Res response.message = f"Error: {e}" return response - def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalResponse: - if feedback is None: - # NOTE: it takes an iteration for the feedback to get set. + def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) -> GoalResponse: + if feedback.status.value != feedback.status.STATUS_PROCESSING: return GoalResponse.IN_PROGRESS - - if feedback.command.command_choice == feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET: - full_body_feedback = feedback.command.full_body_feedback - if full_body_feedback.status.value != full_body_feedback.status.STATUS_PROCESSING: - return GoalResponse.IN_PROGRESS - if full_body_feedback.feedback.feedback_choice == full_body_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: + + match feedback.feedback.feedback_choice: + case feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: return GoalResponse.SUCCESS - elif ( - full_body_feedback.feedback.feedback_choice == full_body_feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET - ): + case feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET: return GoalResponse.SUCCESS - elif ( - full_body_feedback.feedback.feedback_choice - == full_body_feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET - ): + case feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET: if ( - full_body_feedback.feedback.selfright_feedback.status.value - == full_body_feedback.feedback.selfright_feedback.status.STATUS_COMPLETED + feedback.feedback.selfright_feedback.status.value + == feedback.feedback.selfright_feedback.status.STATUS_COMPLETED ): - return GoalResponse.SUCCESS - else: - return GoalResponse.IN_PROGRESS - elif ( - full_body_feedback.feedback.feedback_choice - == full_body_feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET - ): + return GoalResponse.SUCCESS + return GoalResponse.IN_PROGRESS + + case feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET: if ( - full_body_feedback.feedback.safe_power_off_feedback.status.value - == full_body_feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF + feedback.feedback.safe_power_off_feedback.status.value + == feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF ): return GoalResponse.SUCCESS - else: - return GoalResponse.IN_PROGRESS - elif ( - full_body_feedback.feedback.feedback_choice - == full_body_feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET - ): + return GoalResponse.IN_PROGRESS + + case feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET: if ( - full_body_feedback.feedback.battery_change_pose_feedback.status.value - == full_body_feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED + feedback.feedback.battery_change_pose_feedback.status.value + == feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED ): return GoalResponse.SUCCESS if ( - full_body_feedback.feedback.battery_change_pose_feedback.status.value - == full_body_feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED + feedback.feedback.battery_change_pose_feedback.status.value + == feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED ): return GoalResponse.FAILED return GoalResponse.IN_PROGRESS - elif ( - full_body_feedback.feedback.feedback_choice - == full_body_feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET - ): + case feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET: if ( - full_body_feedback.feedback.payload_estimation_feedback.status.value - == full_body_feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED + feedback.feedback.payload_estimation_feedback.status.value + == feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED ): return GoalResponse.SUCCESS - if ( - full_body_feedback.feedback.payload_estimation_feedback.status.value - == full_body_feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS + if (feedback.feedback.payload_estimation_feedback.status.value + == feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS ): return GoalResponse.SUCCESS - if ( - full_body_feedback.feedback.payload_estimation_feedback.status.value - == full_body_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR + if (feedback.feedback.payload_estimation_feedback.status.value + == feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR ): return GoalResponse.FAILED return GoalResponse.IN_PROGRESS - elif ( - full_body_feedback.feedback.feedback_choice - == full_body_feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET - ): - if ( - full_body_feedback.feedback.constrained_manipulation_feedback.status.value - == full_body_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING + case feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET: + if (feedback.feedback.constrained_manipulation_feedback.status.value + == feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING ): return GoalResponse.IN_PROGRESS return GoalResponse.FAILED - else: + case _: return GoalResponse.IN_PROGRESS - elif feedback.command.command_choice == feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET: - # The idea here is that a synchronized command can have arm, mobility, and/or gripper - # sub-commands in it. The total command isn't done until all sub-commands are satisfied. - # So if any one of the sub-commands is still in progress, it short-circuits out as - # IN_PROGRESS. And if it makes it to the bottom of the function, then all components - # must be satisfied, and it returns SUCCESS. - # One corner case to know about is that the commands that don't provide feedback, such - # as a velocity command will return SUCCESS. This allows you to use them more effectively - # with other commands. For example if you wanted to move the arm with some velocity - # while the mobility is going to some SE2 trajectory then that will work. - - sync_feedback = feedback.command.synchronized_feedback - if sync_feedback.arm_command_feedback_is_set is True: - arm_feedback = sync_feedback.arm_command_feedback + def _process_synchronized_arm_command_feedback( + self, feedback: ArmCommandFeedback) -> Optional[GoalResponse]: + if ( + feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN + or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT + or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN + or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ): + return GoalResponse.FAILED + + match feedback.feedback.feedback_choice: + case feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: if ( - arm_feedback.status.value == arm_feedback.status.STATUS_COMMAND_OVERRIDDEN - or arm_feedback.status.value == arm_feedback.status.STATUS_COMMAND_TIMED_OUT - or arm_feedback.status.value == arm_feedback.status.STATUS_ROBOT_FROZEN - or arm_feedback.status.value == arm_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + feedback.feedback.arm_cartesian_feedback.status.value + != feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE ): - return GoalResponse.FAILED - if arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: - if ( - arm_feedback.feedback.arm_cartesian_feedback.status.value - != arm_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE - ): - return GoalResponse.IN_PROGRESS - elif ( - arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET + return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET: + if ( + feedback.feedback.arm_joint_move_feedback.status.value + != feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE ): - if ( - arm_feedback.feedback.arm_joint_move_feedback.status.value - != arm_feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE - ): - return GoalResponse.IN_PROGRESS - elif ( - arm_feedback.feedback.feedback_choice - == arm_feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET + return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET: + if ( + feedback.feedback.named_arm_position_feedback.status.value + != feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE ): - if ( - arm_feedback.feedback.named_arm_position_feedback.status.value - != arm_feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE - ): - return GoalResponse.IN_PROGRESS - elif arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET: - self.get_logger().warn("WARNING: ArmVelocityCommand provides no feedback") - pass # May return SUCCESS below - elif arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET: - if ( - arm_feedback.feedback.arm_gaze_feedback.status.value - != arm_feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE - ): - return GoalResponse.IN_PROGRESS - elif arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET: - self.get_logger().warn("WARNING: Stop command provides no feedback") - pass # May return SUCCESS below - elif arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET: - if ( - arm_feedback.feedback.arm_drag_feedback.status.value - != arm_feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING - ): - return GoalResponse.FAILED - elif arm_feedback.feedback.feedback_choice == arm_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: - self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") - pass # May return SUCCESS below - else: - self.get_logger().error("ERROR: unknown arm command type") return GoalResponse.IN_PROGRESS - - if sync_feedback.mobility_command_feedback_is_set is True: - mob_feedback = sync_feedback.mobility_command_feedback + case feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET: + self.get_logger().warn("WARNING: ArmVelocityCommand provides no feedback") + case feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET: + if ( + feedback.feedback.arm_gaze_feedback.status.value + != feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE + ): + return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET: + self.get_logger().warn("WARNING: Stop command provides no feedback") + case feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET: if ( - mob_feedback.status.value == mob_feedback.status.STATUS_COMMAND_OVERRIDDEN - or mob_feedback.status.value == mob_feedback.status.STATUS_COMMAND_TIMED_OUT - or mob_feedback.status.value == mob_feedback.status.STATUS_ROBOT_FROZEN - or mob_feedback.status.value == mob_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + feedback.feedback.arm_drag_feedback.status.value + != feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING ): return GoalResponse.FAILED - if mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET: - if ( - mob_feedback.feedback.se2_trajectory_feedback.status.value - != mob_feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL - ): - return GoalResponse.IN_PROGRESS - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET: - self.get_logger().warn("WARNING: Planar velocity commands provide no feedback") - pass # May return SUCCESS below - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET: - if ( - mob_feedback.feedback.sit_feedback.status.value - != mob_feedback.feedback.sit_feedback.status.STATUS_IS_SITTING - ): - return GoalResponse.IN_PROGRESS - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET: - if ( - mob_feedback.feedback.stand_feedback.status.value - != mob_feedback.feedback.stand_feedback.status.STATUS_IS_STANDING - ): - return GoalResponse.IN_PROGRESS - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET: - if ( - mob_feedback.feedback.stance_feedback.status.value - == mob_feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY - ): - return GoalResponse.FAILED - if ( - mob_feedback.feedback.stance_feedback.status.value - != mob_feedback.feedback.stance_feedback.status.STATUS_STANCED - ): - return GoalResponse.IN_PROGRESS - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: - self.get_logger().warn("WARNING: Stop command provides no feedback") - pass # May return SUCCESS below - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET: - self.get_logger().warn("WARNING: FollowArmCommand provides no feedback") - pass # May return SUCCESS below - elif mob_feedback.feedback.feedback_choice == mob_feedback.feedback.FEEDBACK_NOT_SET: - # sync_feedback.mobility_command_feedback_is_set, feedback_choice is actually not set. - # This may happen when a command finishes, which means we may return SUCCESS below. - self.get_logger().info("mobility command feedback indicates goal has reached") - pass - else: - self.get_logger().error("ERROR: unknown mobility command type") + case feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: + self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") + case _: + self.get_logger().error("ERROR: unknown arm command type") + return GoalResponse.IN_PROGRESS + return None + + def _process_synchronized_mobility_command_feedback( + self, feedback: MobilityCommandFeedback) -> Optional[GoalResponse]: + if ( + feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN + or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT + or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN + or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ): + return GoalResponse.FAILED + match feedback.feedback.feedback_choice: + case feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET: + if ( + feedback.feedback.se2_trajectory_feedback.status.value + != feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL + ): return GoalResponse.IN_PROGRESS - - if sync_feedback.gripper_command_feedback_is_set is True: - grip_feedback = sync_feedback.gripper_command_feedback + case feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET: + self.get_logger().warn("WARNING: Planar velocity commands provide no feedback") + case feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET: + if ( + feedback.feedback.sit_feedback.status.value + != feedback.feedback.sit_feedback.status.STATUS_IS_SITTING + ): + return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET: + if ( + feedback.feedback.stand_feedback.status.value + != feedback.feedback.stand_feedback.status.STATUS_IS_STANDING + ): + return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET: if ( - grip_feedback.status.value == grip_feedback.status.STATUS_COMMAND_OVERRIDDEN - or grip_feedback.status.value == grip_feedback.status.STATUS_COMMAND_TIMED_OUT - or grip_feedback.status.value == grip_feedback.status.STATUS_ROBOT_FROZEN - or grip_feedback.status.value == grip_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + feedback.feedback.stance_feedback.status.value + == feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY ): return GoalResponse.FAILED - if grip_feedback.command.command_choice == grip_feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET: - if ( - grip_feedback.command.claw_gripper_feedback.status.value - == grip_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS - ): - return GoalResponse.IN_PROGRESS - elif ( - grip_feedback.command.claw_gripper_feedback.status.value - == grip_feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN - ): - self.get_logger().error("ERROR: claw grippper status unknown") - return GoalResponse.IN_PROGRESS - # else: STATUS_AT_GOAL or STATUS_APPLYING_FORCE - - else: - self.get_logger().error("ERROR: unknown gripper command type") + if ( + feedback.feedback.stance_feedback.status.value + != feedback.feedback.stance_feedback.status.STATUS_STANCED + ): return GoalResponse.IN_PROGRESS + case feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: + self.get_logger().warn("WARNING: Stop command provides no feedback") + case feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET: + self.get_logger().warn("WARNING: FollowArmCommand provides no feedback") + case feedback.feedback.FEEDBACK_NOT_SET: + # sync_feedback.mobility_command_feedback_is_set, feedback_choice is actually not set. + # This may happen when a command finishes, which means we may return SUCCESS below. + self.get_logger().info("mobility command feedback indicates goal has reached") + case _: + self.get_logger().error("ERROR: unknown mobility command type") + return GoalResponse.IN_PROGRESS + return None + + def _process_synchronized_gripper_command_feedback( + self, feedback: GripperCommandFeedback) -> Optional[GoalResponse]: + if ( + feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN + or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT + or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN + or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ): + return GoalResponse.FAILED + + match feedback.command.command_choice: + case feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET: + if ( + feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + ): + return GoalResponse.IN_PROGRESS + if ( + feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + ): + self.get_logger().error("ERROR: claw grippper status unknown") + return GoalResponse.IN_PROGRESS + # else: STATUS_AT_GOAL or STATUS_APPLYING_FORCE - return GoalResponse.SUCCESS - - else: - self.get_logger().error("ERROR: unknown robot command type") + case _: + self.get_logger().error("ERROR: unknown gripper command type") + return GoalResponse.IN_PROGRESS + return None + + def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalResponse: + if feedback is None: + # NOTE: it takes an iteration for the feedback to get set. return GoalResponse.IN_PROGRESS + + match feedback.command.command_choice: + case feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET: + full_body_feedback = feedback.command.full_body_feedback + return self._process_full_body_command_feedback(full_body_feedback) + + case feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET: + # The idea here is that a synchronized command can have arm, mobility, and/or gripper + # sub-commands in it. The total command isn't done until all sub-commands are satisfied. + # So if any one of the sub-commands is still in progress, it short-circuits out as + # IN_PROGRESS. And if it makes it to the bottom of the function, then all components + # must be satisfied, and it returns SUCCESS. + # One corner case to know about is that the commands that don't provide feedback, such + # as a velocity command will return SUCCESS. This allows you to use them more effectively + # with other commands. For example if you wanted to move the arm with some velocity + # while the mobility is going to some SE2 trajectory then that will work. + + sync_feedback = feedback.command.synchronized_feedback + if sync_feedback.arm_command_feedback_is_set is True: + arm_feedback = sync_feedback.arm_command_feedback + response = self._process_synchronized_arm_command_feedback(arm_feedback) + if response: + return response + + if sync_feedback.mobility_command_feedback_is_set is True: + mob_feedback = sync_feedback.mobility_command_feedback + response = self._process_synchronized_mobility_command_feedback(mob_feedback) + if response: + return response + + if sync_feedback.gripper_command_feedback_is_set is True: + grip_feedback = sync_feedback.gripper_command_feedback + response = self._process_synchronized_gripper_command_feedback(grip_feedback) + if response: + return response + + return GoalResponse.SUCCESS + + case _: + self.get_logger().error("ERROR: unknown robot command type") + return GoalResponse.IN_PROGRESS def _get_robot_command_feedback(self, goal_id: Optional[str]) -> RobotCommandFeedback: feedback = RobotCommandFeedback() From c33b9e2552b0b9a6111d42f36e10c385e723d3eb Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 19 Oct 2023 18:24:20 -0600 Subject: [PATCH 04/19] no pattern matching --- spot_driver/spot_driver/spot_ros2.py | 355 ++++++++++++--------------- 1 file changed, 156 insertions(+), 199 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index a792ffd97..233086cb7 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1473,66 +1473,49 @@ def handle_max_vel(self, request: SetVelocity.Request, response: SetVelocity.Res def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) -> GoalResponse: if feedback.status.value != feedback.status.STATUS_PROCESSING: return GoalResponse.IN_PROGRESS - - match feedback.feedback.feedback_choice: - case feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: + + fb = feedback.feedback + choice = fb.feedback_choice + + if choice == fb.FEEDBACK_STOP_FEEDBACK_SET: + return GoalResponse.SUCCESS + elif choice == fb.FEEDBACK_FREEZE_FEEDBACK_SET: + return GoalResponse.SUCCESS + elif choice == fb.FEEDBACK_SELFRIGHT_FEEDBACK_SET: + if fb.selfright_feedback.status.value == fb.selfright_feedback.status.STATUS_COMPLETED: return GoalResponse.SUCCESS - case feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET: + return GoalResponse.IN_PROGRESS + + elif choice == fb.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET: + if fb.safe_power_off_feedback.status.value == fb.safe_power_off_feedback.status.STATUS_POWERED_OFF: return GoalResponse.SUCCESS - case feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET: - if ( - feedback.feedback.selfright_feedback.status.value - == feedback.feedback.selfright_feedback.status.STATUS_COMPLETED - ): - return GoalResponse.SUCCESS - return GoalResponse.IN_PROGRESS - - case feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET: - if ( - feedback.feedback.safe_power_off_feedback.status.value - == feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF - ): - return GoalResponse.SUCCESS - return GoalResponse.IN_PROGRESS - - case feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET: - if ( - feedback.feedback.battery_change_pose_feedback.status.value - == feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED - ): - return GoalResponse.SUCCESS - if ( - feedback.feedback.battery_change_pose_feedback.status.value - == feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED - ): - return GoalResponse.FAILED - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET: - if ( - feedback.feedback.payload_estimation_feedback.status.value - == feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED - ): - return GoalResponse.SUCCESS - if (feedback.feedback.payload_estimation_feedback.status.value - == feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS - ): - return GoalResponse.SUCCESS - if (feedback.feedback.payload_estimation_feedback.status.value - == feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR - ): - return GoalResponse.FAILED - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET: - if (feedback.feedback.constrained_manipulation_feedback.status.value - == feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING - ): - return GoalResponse.IN_PROGRESS + return GoalResponse.IN_PROGRESS + + elif choice == fb.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET: + if fb.battery_change_pose_feedback.status.value == fb.battery_change_pose_feedback.status.STATUS_COMPLETED: + return GoalResponse.SUCCESS + if fb.battery_change_pose_feedback.status.value == fb.battery_change_pose_feedback.status.STATUS_FAILED: return GoalResponse.FAILED - case _: + return GoalResponse.IN_PROGRESS + + elif choice == fb.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET: + if fb.payload_estimation_feedback.status.value == fb.payload_estimation_feedback.status.STATUS_COMPLETED: + return GoalResponse.SUCCESS + if fb.payload_estimation_feedback.status.value == fb.payload_estimation_feedback.status.STATUS_SMALL_MASS: + return GoalResponse.SUCCESS + if fb.payload_estimation_feedback.status.value == fb.payload_estimation_feedback.status.STATUS_ERROR: + return GoalResponse.FAILED + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET: + if ( + fb.constrained_manipulation_feedback.status.value + == fb.constrained_manipulation_feedback.status.STATUS_RUNNING + ): return GoalResponse.IN_PROGRESS + return GoalResponse.FAILED + return GoalResponse.IN_PROGRESS - def _process_synchronized_arm_command_feedback( - self, feedback: ArmCommandFeedback) -> Optional[GoalResponse]: + def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedback) -> Optional[GoalResponse]: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1540,51 +1523,39 @@ def _process_synchronized_arm_command_feedback( or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE ): return GoalResponse.FAILED - - match feedback.feedback.feedback_choice: - case feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: - if ( - feedback.feedback.arm_cartesian_feedback.status.value - != feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET: - if ( - feedback.feedback.arm_joint_move_feedback.status.value - != feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET: - if ( - feedback.feedback.named_arm_position_feedback.status.value - != feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET: - self.get_logger().warn("WARNING: ArmVelocityCommand provides no feedback") - case feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET: - if ( - feedback.feedback.arm_gaze_feedback.status.value - != feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET: - self.get_logger().warn("WARNING: Stop command provides no feedback") - case feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET: - if ( - feedback.feedback.arm_drag_feedback.status.value - != feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING - ): - return GoalResponse.FAILED - case feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: - self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") - case _: - self.get_logger().error("ERROR: unknown arm command type") + + fb = feedback.feedback + choice = fb.feedback_choice + + if choice == fb.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: + if fb.arm_cartesian_feedback.status.value != fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET: + if fb.arm_joint_move_feedback.status.value != fb.arm_joint_move_feedback.status.STATUS_COMPLETE: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET: + if fb.named_arm_position_feedback.status.value != fb.named_arm_position_feedback.status.STATUS_COMPLETE: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET: + self.get_logger().warn("WARNING: ArmVelocityCommand provides no feedback") + elif choice == fb.FEEDBACK_ARM_GAZE_FEEDBACK_SET: + if fb.arm_gaze_feedback.status.value != fb.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE: return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_ARM_STOP_FEEDBACK_SET: + self.get_logger().warn("WARNING: Stop command provides no feedback") + elif choice == fb.FEEDBACK_ARM_DRAG_FEEDBACK_SET: + if fb.arm_drag_feedback.status.value != fb.arm_drag_feedback.status.STATUS_DRAGGING: + return GoalResponse.FAILED + elif choice == fb.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: + self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") + else: + self.get_logger().error("ERROR: unknown arm command type") + return GoalResponse.IN_PROGRESS return None - + def _process_synchronized_mobility_command_feedback( - self, feedback: MobilityCommandFeedback) -> Optional[GoalResponse]: + self, feedback: MobilityCommandFeedback + ) -> Optional[GoalResponse]: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1592,53 +1563,42 @@ def _process_synchronized_mobility_command_feedback( or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE ): return GoalResponse.FAILED - match feedback.feedback.feedback_choice: - case feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET: - if ( - feedback.feedback.se2_trajectory_feedback.status.value - != feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET: - self.get_logger().warn("WARNING: Planar velocity commands provide no feedback") - case feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET: - if ( - feedback.feedback.sit_feedback.status.value - != feedback.feedback.sit_feedback.status.STATUS_IS_SITTING - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET: - if ( - feedback.feedback.stand_feedback.status.value - != feedback.feedback.stand_feedback.status.STATUS_IS_STANDING - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET: - if ( - feedback.feedback.stance_feedback.status.value - == feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY - ): - return GoalResponse.FAILED - if ( - feedback.feedback.stance_feedback.status.value - != feedback.feedback.stance_feedback.status.STATUS_STANCED - ): - return GoalResponse.IN_PROGRESS - case feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET: - self.get_logger().warn("WARNING: Stop command provides no feedback") - case feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET: - self.get_logger().warn("WARNING: FollowArmCommand provides no feedback") - case feedback.feedback.FEEDBACK_NOT_SET: - # sync_feedback.mobility_command_feedback_is_set, feedback_choice is actually not set. - # This may happen when a command finishes, which means we may return SUCCESS below. - self.get_logger().info("mobility command feedback indicates goal has reached") - case _: - self.get_logger().error("ERROR: unknown mobility command type") + + fb = feedback.feedback + choice = fb.feedback_choice + + if choice == fb.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET: + if fb.se2_trajectory_feedback.status.value != fb.se2_trajectory_feedback.status.STATUS_AT_GOAL: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET: + self.get_logger().warn("WARNING: Planar velocity commands provide no feedback") + elif choice == fb.FEEDBACK_SIT_FEEDBACK_SET: + if fb.sit_feedback.status.value != fb.sit_feedback.status.STATUS_IS_SITTING: return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_STAND_FEEDBACK_SET: + if fb.stand_feedback.status.value != fb.stand_feedback.status.STATUS_IS_STANDING: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_STANCE_FEEDBACK_SET: + if fb.stance_feedback.status.value == fb.stance_feedback.status.STATUS_TOO_FAR_AWAY: + return GoalResponse.FAILED + if fb.stance_feedback.status.value != fb.stance_feedback.status.STATUS_STANCED: + return GoalResponse.IN_PROGRESS + elif choice == fb.FEEDBACK_STOP_FEEDBACK_SET: + self.get_logger().warn("WARNING: Stop command provides no feedback") + elif choice == fb.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET: + self.get_logger().warn("WARNING: FollowArmCommand provides no feedback") + elif choice == fb.FEEDBACK_NOT_SET: + # sync_feedback.mobility_command_feedback_is_set, feedback_choice is actually not set. + # This may happen when a command finishes, which means we may return SUCCESS below. + self.get_logger().info("mobility command feedback indicates goal has reached") + else: + self.get_logger().error("ERROR: unknown mobility command type") + return GoalResponse.IN_PROGRESS return None - + def _process_synchronized_gripper_command_feedback( - self, feedback: GripperCommandFeedback) -> Optional[GoalResponse]: + self, feedback: GripperCommandFeedback + ) -> Optional[GoalResponse]: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1646,72 +1606,69 @@ def _process_synchronized_gripper_command_feedback( or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE ): return GoalResponse.FAILED - - match feedback.command.command_choice: - case feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET: - if ( - feedback.command.claw_gripper_feedback.status.value - == feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS - ): - return GoalResponse.IN_PROGRESS - if ( - feedback.command.claw_gripper_feedback.status.value - == feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN - ): - self.get_logger().error("ERROR: claw grippper status unknown") - return GoalResponse.IN_PROGRESS - # else: STATUS_AT_GOAL or STATUS_APPLYING_FORCE - - case _: - self.get_logger().error("ERROR: unknown gripper command type") + + if feedback.command.command_choice == feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET: + if ( + feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + ): + return GoalResponse.IN_PROGRESS + if ( + feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + ): + self.get_logger().error("ERROR: claw grippper status unknown") return GoalResponse.IN_PROGRESS + # else: STATUS_AT_GOAL or STATUS_APPLYING_FORCE + else: + self.get_logger().error("ERROR: unknown gripper command type") + return GoalResponse.IN_PROGRESS return None - + def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalResponse: if feedback is None: # NOTE: it takes an iteration for the feedback to get set. return GoalResponse.IN_PROGRESS - - match feedback.command.command_choice: - case feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET: - full_body_feedback = feedback.command.full_body_feedback - return self._process_full_body_command_feedback(full_body_feedback) - - case feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET: - # The idea here is that a synchronized command can have arm, mobility, and/or gripper - # sub-commands in it. The total command isn't done until all sub-commands are satisfied. - # So if any one of the sub-commands is still in progress, it short-circuits out as - # IN_PROGRESS. And if it makes it to the bottom of the function, then all components - # must be satisfied, and it returns SUCCESS. - # One corner case to know about is that the commands that don't provide feedback, such - # as a velocity command will return SUCCESS. This allows you to use them more effectively - # with other commands. For example if you wanted to move the arm with some velocity - # while the mobility is going to some SE2 trajectory then that will work. - - sync_feedback = feedback.command.synchronized_feedback - if sync_feedback.arm_command_feedback_is_set is True: - arm_feedback = sync_feedback.arm_command_feedback - response = self._process_synchronized_arm_command_feedback(arm_feedback) - if response: - return response - - if sync_feedback.mobility_command_feedback_is_set is True: - mob_feedback = sync_feedback.mobility_command_feedback - response = self._process_synchronized_mobility_command_feedback(mob_feedback) - if response: - return response - - if sync_feedback.gripper_command_feedback_is_set is True: - grip_feedback = sync_feedback.gripper_command_feedback - response = self._process_synchronized_gripper_command_feedback(grip_feedback) - if response: - return response - return GoalResponse.SUCCESS + choice = feedback.command.command_choice + if choice == feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET: + full_body_feedback = feedback.command.full_body_feedback + return self._process_full_body_command_feedback(full_body_feedback) + + elif choice == feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET: + # The idea here is that a synchronized command can have arm, mobility, and/or gripper + # sub-commands in it. The total command isn't done until all sub-commands are satisfied. + # So if any one of the sub-commands is still in progress, it short-circuits out as + # IN_PROGRESS. And if it makes it to the bottom of the function, then all components + # must be satisfied, and it returns SUCCESS. + # One corner if choice == to know about is that the commands that don't provide feedback, such + # as a velocity command will return SUCCESS. This allows you to use them more effectively + # with other commands. For example if you wanted to move the arm with some velocity + # while the mobility is going to some SE2 trajectory then that will work. + + sync_feedback = feedback.command.synchronized_feedback + if sync_feedback.arm_command_feedback_is_set is True: + arm_feedback = sync_feedback.arm_command_feedback + response = self._process_synchronized_arm_command_feedback(arm_feedback) + if response is not None: + return response + + if sync_feedback.mobility_command_feedback_is_set is True: + mob_feedback = sync_feedback.mobility_command_feedback + response = self._process_synchronized_mobility_command_feedback(mob_feedback) + if response is not None: + return response + + if sync_feedback.gripper_command_feedback_is_set is True: + grip_feedback = sync_feedback.gripper_command_feedback + response = self._process_synchronized_gripper_command_feedback(grip_feedback) + if response is not None: + return response - case _: - self.get_logger().error("ERROR: unknown robot command type") - return GoalResponse.IN_PROGRESS + return GoalResponse.SUCCESS + else: + self.get_logger().error("ERROR: unknown robot command type") + return GoalResponse.IN_PROGRESS def _get_robot_command_feedback(self, goal_id: Optional[str]) -> RobotCommandFeedback: feedback = RobotCommandFeedback() From b55de91cb26564faf67d8f4cc8449bf8e7d231a3 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 19 Oct 2023 18:36:04 -0600 Subject: [PATCH 05/19] tiny comment change --- spot_driver/test/test_spot_driver.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 8922274c0..0aff1ce91 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -266,12 +266,12 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # should these next two test cases be failures? + # Should this status return a GoalResponse.FAILED? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - + # Should this status return a GoalResponse.FAILED? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ) @@ -297,7 +297,7 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? + # Should this status return a GoalResponse.FAILED? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED ) @@ -323,7 +323,7 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? + # Should this status return a GoalResponse.FAILED? feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM ) @@ -356,7 +356,7 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? + # Should this status return a GoalResponse.FAILED? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED ) @@ -619,7 +619,7 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE - ) + # ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing unknown gripper command """ From b3a2697cc2a1a6b8b04b411a479b4468c0c3249e Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 19 Oct 2023 18:38:29 -0600 Subject: [PATCH 06/19] removed a random comment --- spot_driver/test/test_spot_driver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 0aff1ce91..52a388c2b 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -619,7 +619,7 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE - # ) + ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing unknown gripper command """ From 37f917c4ce3de161052f1d05248542c478578898 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Mon, 23 Oct 2023 10:14:19 -0600 Subject: [PATCH 07/19] refactor tests --- spot_driver/test/test_spot_driver.py | 643 ++++++++++++--------------- 1 file changed, 294 insertions(+), 349 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 52a388c2b..cc1504092 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -3,7 +3,13 @@ import bdai_ros2_wrappers.scope as ros_scope import rclpy -from bosdyn_msgs.msg import RobotCommandFeedback +from bosdyn_msgs.msg import ( + ArmCommandFeedback, + FullBodyCommandFeedback, + GripperCommandFeedback, + MobilityCommandFeedback, + RobotCommandFeedback, +) from std_srvs.srv import Trigger import spot_driver.spot_ros2 @@ -12,6 +18,25 @@ Dock, ) +FEEDBACK_INVALID = -128 + +def spin_thread(executor: MultiThreadedExecutor) -> None: + if executor is not None: + try: + executor.spin() + except (ExternalShutdownException, KeyboardInterrupt): + pass + + +def call_trigger_client( + client: rclpy.node.Client, executor: MultiThreadedExecutor, request: Any = Trigger.Request() +) -> spot_driver.spot_ros2.Response: + req = request + future = client.call_async(req) + executor.spin_until_future_complete(future) + resp = future.result() + return resp + class SpotDriverTest(unittest.TestCase): def setUp(self) -> None: @@ -67,564 +92,484 @@ def test_wrapped_commands(self) -> None: resp = self.dock_client.call(Dock.Request()) self.assertEqual(resp.success, True) - # Ignore Line too long errors - # ruff: noqa: E501 - def test_robot_command_goal_complete(self) -> None: - FEEDBACK_INVALID = -128 - - self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) - - feedback = RobotCommandFeedback() - - """ Testing FullBodyFeedback """ - - fullbody_feedback = feedback.command.full_body_feedback - feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET + def test_process_full_body_command_feedback(self) -> None: + feedback = FullBodyCommandFeedback() - feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_UNKNOWN - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + feedback.status.value = feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_PROCESSING + feedback.command.full_body_feedback.status.value = feedback.status.STATUS_PROCESSING """ Testing STOP_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) """ Testing FREEZE_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) """ Testing SELFRIGHT_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET - ) + feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( - fullbody_feedback.feedback.selfright_feedback.status.STATUS_COMPLETED + feedback.feedback.selfright_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( - fullbody_feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN + feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( - fullbody_feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing SAFE_POWER_OFF_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET + feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET ) feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( - fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF + feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( - fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN + feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( - fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing BATTERY_CHANGE_POSE_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET + feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET ) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( - fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED + feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( - fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN + feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( - fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( - fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED + feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) """ Testing PAYLOAD_ESTIMATION_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET + feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET ) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( - fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED + feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( - fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS + feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( - fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR + feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( - fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN + feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( - fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing CONSTRAINED_MANIPULATION_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( - fullbody_feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET + feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET ) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( - fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING + feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( - fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST + feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( - fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK + feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( - fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN + feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) - """ Testing Synchronized Feedback Command """ - arm_command_feedback = feedback.command.synchronized_feedback.arm_command_feedback - feedback.command.command_choice = feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET + def test_process_synchronized_arm_command_feedback(self) -> None: + feedback = ArmCommandFeedback() - """ Testing arm command feedback """ - feedback.command.synchronized_feedback.arm_command_feedback_is_set = True - feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False - feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False + feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN - feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( - arm_command_feedback.status.STATUS_COMMAND_OVERRIDDEN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( - arm_command_feedback.status.STATUS_COMMAND_TIMED_OUT - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( - arm_command_feedback.status.STATUS_ROBOT_FROZEN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( - arm_command_feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( - arm_command_feedback.status.STATUS_PROCESSING - ) + feedback.status.value = feedback.status.STATUS_PROCESSING """ Testing arm cartesian feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( - arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE + feedback.feedback.arm_cartesian_feedback.status.value = ( + feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( - arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN + feedback.feedback.arm_cartesian_feedback.status.value = ( + feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( - arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.arm_cartesian_feedback.status.value = ( + feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) # Should this status return a GoalResponse.FAILED? - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( - arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED + feedback.feedback.arm_cartesian_feedback.status.value = ( + feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) # Should this status return a GoalResponse.FAILED? - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( - arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED + feedback.feedback.arm_cartesian_feedback.status.value = ( + feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing arm joint move feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( - arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE + feedback.feedback.arm_joint_move_feedback.status.value = ( + feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( - arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN + feedback.feedback.arm_joint_move_feedback.status.value = ( + feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( - arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.arm_joint_move_feedback.status.value = ( + feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) # Should this status return a GoalResponse.FAILED? - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( - arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED + feedback.feedback.arm_joint_move_feedback.status.value = ( + feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing named arm position feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET - feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( - arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE + feedback.feedback.named_arm_position_feedback.status.value = ( + feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( - arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN + feedback.feedback.named_arm_position_feedback.status.value = ( + feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( - arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.named_arm_position_feedback.status.value = ( + feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) # Should this status return a GoalResponse.FAILED? - feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( - arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM + feedback.feedback.named_arm_position_feedback.status.value = ( + feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing arm velocity feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET - ) - # Arm velocity commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) """ Testing arm gaze feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( - arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE + feedback.feedback.arm_gaze_feedback.status.value = ( + feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( - arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + feedback.feedback.arm_gaze_feedback.status.value = feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( - arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + feedback.feedback.arm_gaze_feedback.status.value = feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) # Should this status return a GoalResponse.FAILED? - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( - arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED + feedback.feedback.arm_gaze_feedback.status.value = ( + feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing arm stop feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET - ) - # Arm stop commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET + + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) """ Testing arm drag feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( - arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( - arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( - arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED + feedback.feedback.arm_drag_feedback.status.value = ( + feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( - arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE + feedback.feedback.arm_drag_feedback.status.value = ( + feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) """ Testing arm drag feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET - ) - # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET - """ Testing arm drag feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET - ) - # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + + """ Testing arm Impedance feedback """ + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET + + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) """ Testing unknown arm command """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID - # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + feedback.feedback.feedback_choice = FEEDBACK_INVALID - """ Testing mobility commands """ - mobility_feedback = RobotCommandFeedback().command.synchronized_feedback.mobility_command_feedback + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.arm_command_feedback_is_set = False - feedback.command.synchronized_feedback.mobility_command_feedback_is_set = True - feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False + def test_process_synchronized_mobility_command_feedback(self) -> None: + """Testing mobility commands""" + feedback = MobilityCommandFeedback() - feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( - mobility_feedback.status.STATUS_COMMAND_OVERRIDDEN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( - mobility_feedback.status.STATUS_COMMAND_TIMED_OUT - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( - mobility_feedback.status.STATUS_ROBOT_FROZEN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( - mobility_feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( - mobility_feedback.status.STATUS_PROCESSING - ) + feedback.status.value = feedback.status.STATUS_PROCESSING """ Testing se2 trajectory feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( - mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL + feedback.feedback.se2_trajectory_feedback.status.value = ( + feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( - mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN + feedback.feedback.se2_trajectory_feedback.status.value = ( + feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( - mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL + feedback.feedback.se2_trajectory_feedback.status.value = ( + feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL + ) + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( - mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL + feedback.feedback.se2_trajectory_feedback.status.value = ( + feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL + ) + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing se2 velocity feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET - ) - # Planar velocity commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) """ Testing sit feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( - mobility_feedback.feedback.sit_feedback.status.STATUS_IS_SITTING - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_IS_SITTING + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( - mobility_feedback.feedback.sit_feedback.status.STATUS_UNKNOWN + feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_UNKNOWN + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( - mobility_feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stand feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( - mobility_feedback.feedback.stand_feedback.status.STATUS_IS_STANDING - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_IS_STANDING + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( - mobility_feedback.feedback.stand_feedback.status.STATUS_UNKNOWN + feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_UNKNOWN + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( - mobility_feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS + feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stance feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET - ) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( - mobility_feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( - mobility_feedback.feedback.stance_feedback.status.STATUS_STANCED - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_STANCED + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( - mobility_feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE + feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( - mobility_feedback.feedback.stance_feedback.status.STATUS_UNKNOWN + feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_UNKNOWN + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stop feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET - ) - # Stop commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET - """ Testing stop feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET - ) - # follow arm commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) - """ Testing stop feedback """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( - mobility_feedback.feedback.FEEDBACK_NOT_SET - ) - # mobility command feedback is not set, this could be caused by a command that finishes and resets the feedback status. - # because of this case, it will return success as long as no other synchronous commands are run afterwards. - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + """ Testing Follow arm feedback """ + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET - """ Testing unknown command """ - feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) - """ Testing Gripper commands """ - gripper_feedback = RobotCommandFeedback().command.synchronized_feedback.gripper_command_feedback + """ Testing feedback not set""" + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET - feedback.command.synchronized_feedback.arm_command_feedback_is_set = False - feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False - feedback.command.synchronized_feedback.gripper_command_feedback_is_set = True + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) - feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( - gripper_feedback.status.STATUS_COMMAND_OVERRIDDEN + """ Testing unknown command """ + feedback.feedback.feedback_choice = FEEDBACK_INVALID + self.assertEqual( + self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( - gripper_feedback.status.STATUS_COMMAND_TIMED_OUT - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + def test_process_synchronized_gripper_command_feedback(self) -> None: + """Testing Gripper commands""" + feedback = GripperCommandFeedback() - feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( - gripper_feedback.status.STATUS_ROBOT_FROZEN - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( - gripper_feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( - gripper_feedback.status.STATUS_PROCESSING - ) + feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) + + feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) + + feedback.status.value = feedback.status.STATUS_PROCESSING """ Testing Claw Gripper feedback """ - feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = ( - gripper_feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET + feedback.command.command_choice = feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET + feedback.command.claw_gripper_feedback.status.value = ( + feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS ) - feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( - gripper_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + self.assertEqual( + self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( - gripper_feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + feedback.command.claw_gripper_feedback.status.value = ( + feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual( + self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( - gripper_feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL + feedback.command.claw_gripper_feedback.status.value = ( + feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) - feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( - gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE + feedback.command.claw_gripper_feedback.status.value = ( + feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) + + """ Testing feedback not set""" + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET + + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) """ Testing unknown gripper command """ - feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = FEEDBACK_INVALID - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + feedback.command.command_choice = FEEDBACK_INVALID + self.assertEqual( + self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS + ) + + def test_robot_command_goal_complete(self) -> None: + self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) + + feedback = RobotCommandFeedback() + + feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET + + """ Testing Synchronized Feedback Command """ + + feedback.command.command_choice = feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET """ Testing unknown robot command type """ feedback.command.command_choice = FEEDBACK_INVALID From e19b97b918cac9a86ce457d2e541057740bd15f6 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Mon, 23 Oct 2023 17:41:04 +0000 Subject: [PATCH 08/19] fix test issues --- spot_driver/test/test_spot_driver.py | 78 ++++++++++++++-------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index cc1504092..3ef78d244 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -98,125 +98,125 @@ def test_process_full_body_command_feedback(self) -> None: feedback.status.value = feedback.status.STATUS_UNKNOWN self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.status.value = feedback.status.STATUS_PROCESSING + feedback.status.value = feedback.status.STATUS_PROCESSING """ Testing STOP_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) """ Testing FREEZE_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) """ Testing SELFRIGHT_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET - feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET + feedback.feedback.selfright_feedback.status.value = ( feedback.feedback.selfright_feedback.status.STATUS_COMPLETED ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + feedback.feedback.selfright_feedback.status.value = ( feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + feedback.feedback.selfright_feedback.status.value = ( feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing SAFE_POWER_OFF_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( + feedback.feedback.feedback_choice = ( feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET ) - feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + feedback.feedback.safe_power_off_feedback.status.value = ( feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + feedback.feedback.safe_power_off_feedback.status.value = ( feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + feedback.feedback.safe_power_off_feedback.status.value = ( feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing BATTERY_CHANGE_POSE_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( + feedback.feedback.feedback_choice = ( feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET ) - feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + feedback.feedback.battery_change_pose_feedback.status.value = ( feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + feedback.feedback.battery_change_pose_feedback.status.value = ( feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + feedback.feedback.battery_change_pose_feedback.status.value = ( feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + feedback.feedback.battery_change_pose_feedback.status.value = ( feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) """ Testing PAYLOAD_ESTIMATION_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( + feedback.feedback.feedback_choice = ( feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET ) - feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + feedback.feedback.payload_estimation_feedback.status.value = ( feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + feedback.feedback.payload_estimation_feedback.status.value = ( feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) - feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + feedback.feedback.payload_estimation_feedback.status.value = ( feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + feedback.feedback.payload_estimation_feedback.status.value = ( feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + feedback.feedback.payload_estimation_feedback.status.value = ( feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing CONSTRAINED_MANIPULATION_FEEDBACK_SET """ - feedback.command.full_body_feedback.feedback.feedback_choice = ( + feedback.feedback.feedback_choice = ( feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET ) - feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + feedback.feedback.constrained_manipulation_feedback.status.value = ( feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) - feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + feedback.feedback.constrained_manipulation_feedback.status.value = ( feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + feedback.feedback.constrained_manipulation_feedback.status.value = ( feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) - feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + feedback.feedback.constrained_manipulation_feedback.status.value = ( feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN ) self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) @@ -245,7 +245,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.arm_cartesian_feedback.status.value = ( feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) feedback.feedback.arm_cartesian_feedback.status.value = ( feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN @@ -274,7 +274,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.arm_joint_move_feedback.status.value = ( feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) feedback.feedback.arm_joint_move_feedback.status.value = ( feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN @@ -298,7 +298,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.named_arm_position_feedback.status.value = ( feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) feedback.feedback.named_arm_position_feedback.status.value = ( feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN @@ -326,7 +326,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.arm_gaze_feedback.status.value = ( feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) feedback.feedback.arm_gaze_feedback.status.value = feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) @@ -349,7 +349,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) @@ -375,7 +375,7 @@ def test_process_synchronized_arm_command_feedback(self) -> None: feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing unknown arm command """ feedback.feedback.feedback_choice = FEEDBACK_INVALID @@ -406,7 +406,7 @@ def test_process_synchronized_mobility_command_feedback(self) -> None: feedback.feedback.se2_trajectory_feedback.status.value = ( feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) feedback.feedback.se2_trajectory_feedback.status.value = ( feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN @@ -437,7 +437,7 @@ def test_process_synchronized_mobility_command_feedback(self) -> None: feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_IS_SITTING - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_UNKNOWN self.assertEqual( @@ -453,7 +453,7 @@ def test_process_synchronized_mobility_command_feedback(self) -> None: feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_IS_STANDING - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_UNKNOWN self.assertEqual( @@ -472,7 +472,7 @@ def test_process_synchronized_mobility_command_feedback(self) -> None: self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_STANCED - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE self.assertEqual( @@ -550,9 +550,9 @@ def test_process_synchronized_gripper_command_feedback(self) -> None: self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) """ Testing feedback not set""" - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET + feedback.command.command_choice = feedback.command.COMMAND_NOT_SET - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS) """ Testing unknown gripper command """ feedback.command.command_choice = FEEDBACK_INVALID From e3b12e948c1ce4e7d32c74e08ee55c40245b151e Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Mon, 23 Oct 2023 11:52:29 -0600 Subject: [PATCH 09/19] revert test to pre-refactor state --- spot_driver/test/test_spot_driver.py | 676 +++++++++++++++------------ 1 file changed, 376 insertions(+), 300 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 3ef78d244..c0946ab6c 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -4,10 +4,6 @@ import bdai_ros2_wrappers.scope as ros_scope import rclpy from bosdyn_msgs.msg import ( - ArmCommandFeedback, - FullBodyCommandFeedback, - GripperCommandFeedback, - MobilityCommandFeedback, RobotCommandFeedback, ) from std_srvs.srv import Trigger @@ -92,484 +88,564 @@ def test_wrapped_commands(self) -> None: resp = self.dock_client.call(Dock.Request()) self.assertEqual(resp.success, True) - def test_process_full_body_command_feedback(self) -> None: - feedback = FullBodyCommandFeedback() + # Ignore Line too long errors + # ruff: noqa: E501 + def test_robot_command_goal_complete(self) -> None: + FEEDBACK_INVALID = -128 + + self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) + + feedback = RobotCommandFeedback() + + """ Testing FullBodyFeedback """ - feedback.status.value = feedback.status.STATUS_UNKNOWN - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + fullbody_feedback = feedback.command.full_body_feedback + feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET - feedback.status.value = feedback.status.STATUS_PROCESSING + feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_UNKNOWN + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_PROCESSING """ Testing STOP_FEEDBACK_SET """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing FREEZE_FEEDBACK_SET """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing SELFRIGHT_FEEDBACK_SET """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET - feedback.feedback.selfright_feedback.status.value = ( - feedback.feedback.selfright_feedback.status.STATUS_COMPLETED + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET + ) + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.selfright_feedback.status.value = ( - feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.selfright_feedback.status.value = ( - feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS + feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( + fullbody_feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing SAFE_POWER_OFF_FEEDBACK_SET """ - feedback.feedback.feedback_choice = ( - feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET ) - feedback.feedback.safe_power_off_feedback.status.value = ( - feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.safe_power_off_feedback.status.value = ( - feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.safe_power_off_feedback.status.value = ( - feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS + feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( + fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing BATTERY_CHANGE_POSE_FEEDBACK_SET """ - feedback.feedback.feedback_choice = ( - feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET ) - feedback.feedback.battery_change_pose_feedback.status.value = ( - feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.battery_change_pose_feedback.status.value = ( - feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.battery_change_pose_feedback.status.value = ( - feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.battery_change_pose_feedback.status.value = ( - feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED + feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( + fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing PAYLOAD_ESTIMATION_FEEDBACK_SET """ - feedback.feedback.feedback_choice = ( - feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET ) - feedback.feedback.payload_estimation_feedback.status.value = ( - feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.payload_estimation_feedback.status.value = ( - feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.SUCCESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.payload_estimation_feedback.status.value = ( - feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.payload_estimation_feedback.status.value = ( - feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.payload_estimation_feedback.status.value = ( - feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS + feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( + fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing CONSTRAINED_MANIPULATION_FEEDBACK_SET """ - feedback.feedback.feedback_choice = ( - feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET + feedback.command.full_body_feedback.feedback.feedback_choice = ( + fullbody_feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET ) - feedback.feedback.constrained_manipulation_feedback.status.value = ( - feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.constrained_manipulation_feedback.status.value = ( - feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.constrained_manipulation_feedback.status.value = ( - feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.constrained_manipulation_feedback.status.value = ( - feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN + feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( + fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_full_body_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - def test_process_synchronized_arm_command_feedback(self) -> None: - feedback = ArmCommandFeedback() + """ Testing Synchronized Feedback Command """ + arm_command_feedback = feedback.command.synchronized_feedback.arm_command_feedback + feedback.command.command_choice = feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET - feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN + """ Testing arm command feedback """ + feedback.command.synchronized_feedback.arm_command_feedback_is_set = True + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_COMMAND_OVERRIDDEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_PROCESSING + feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( + arm_command_feedback.status.STATUS_PROCESSING + ) """ Testing arm cartesian feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET + ) - feedback.feedback.arm_cartesian_feedback.status.value = ( - feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.arm_cartesian_feedback.status.value = ( - feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.arm_cartesian_feedback.status.value = ( - feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this status return a GoalResponse.FAILED? - feedback.feedback.arm_cartesian_feedback.status.value = ( - feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED + # should these next two test cases be failures? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) - # Should this status return a GoalResponse.FAILED? - feedback.feedback.arm_cartesian_feedback.status.value = ( - feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( + arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing arm joint move feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET + ) - feedback.feedback.arm_joint_move_feedback.status.value = ( - feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.arm_joint_move_feedback.status.value = ( - feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.arm_joint_move_feedback.status.value = ( - feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this status return a GoalResponse.FAILED? - feedback.feedback.arm_joint_move_feedback.status.value = ( - feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( + arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing named arm position feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET + ) - feedback.feedback.named_arm_position_feedback.status.value = ( - feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.named_arm_position_feedback.status.value = ( - feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.named_arm_position_feedback.status.value = ( - feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this status return a GoalResponse.FAILED? - feedback.feedback.named_arm_position_feedback.status.value = ( - feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( + arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing arm velocity feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET + ) + # Arm velocity commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing arm gaze feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_GAZE_FEEDBACK_SET + ) - feedback.feedback.arm_gaze_feedback.status.value = ( - feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.arm_gaze_feedback.status.value = feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.arm_gaze_feedback.status.value = feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this status return a GoalResponse.FAILED? - feedback.feedback.arm_gaze_feedback.status.value = ( - feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED + # Should this test case be a Failure? + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( + arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing arm stop feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET - - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET + ) + # Arm stop commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing arm drag feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_DRAG_FEEDBACK_SET + ) - feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.arm_drag_feedback.status.value = feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.arm_drag_feedback.status.value = ( - feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.arm_drag_feedback.status.value = ( - feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( + arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE ) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.FAILED) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing arm drag feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET - - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) - - """ Testing arm Impedance feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), None) - - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + ) + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + """ Testing arm drag feedback """ + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( + arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + ) + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing unknown arm command """ - feedback.feedback.feedback_choice = FEEDBACK_INVALID + feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID + # Arm impedance commands do not provide feedback therefore we should get a success + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - self.assertEqual(self.spot_ros2._process_synchronized_arm_command_feedback(feedback), GoalResponse.IN_PROGRESS) + """ Testing mobility commands """ + mobility_feedback = RobotCommandFeedback().command.synchronized_feedback.mobility_command_feedback - def test_process_synchronized_mobility_command_feedback(self) -> None: - """Testing mobility commands""" - feedback = MobilityCommandFeedback() + feedback.command.synchronized_feedback.arm_command_feedback_is_set = False + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = True + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = False - feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_COMMAND_OVERRIDDEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_PROCESSING + feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( + mobility_feedback.status.STATUS_PROCESSING + ) """ Testing se2 trajectory feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET - - feedback.feedback.se2_trajectory_feedback.status.value = ( - feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET ) - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) - feedback.feedback.se2_trajectory_feedback.status.value = ( - feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN - ) - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.se2_trajectory_feedback.status.value = ( - feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL - ) - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.se2_trajectory_feedback.status.value = ( - feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL ) - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( + mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing se2 velocity feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET + ) + # Planar velocity commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing sit feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_SIT_FEEDBACK_SET + ) - feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_IS_SITTING - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_IS_SITTING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_UNKNOWN - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_UNKNOWN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.sit_feedback.status.value = feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( + mobility_feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stand feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STAND_FEEDBACK_SET + ) - feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_IS_STANDING - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_IS_STANDING + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_UNKNOWN - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_UNKNOWN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.stand_feedback.status.value = feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( + mobility_feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stance feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STANCE_FEEDBACK_SET + ) - feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_STANCED - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_STANCED + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.feedback.stance_feedback.status.value = feedback.feedback.stance_feedback.status.STATUS_UNKNOWN - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( + mobility_feedback.feedback.stance_feedback.status.STATUS_UNKNOWN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing stop feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET + ) + # Stop commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + """ Testing stop feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET + ) + # follow arm commands provide no feedback, therefore expect SUCCESS + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - """ Testing Follow arm feedback """ - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET + """ Testing stop feedback """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( + mobility_feedback.feedback.FEEDBACK_NOT_SET + ) + # mobility command feedback is not set, this could be caused by a command that finishes and resets the feedback status. + # because of this case, it will return success as long as no other synchronous commands are run afterwards. + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + """ Testing unknown command """ + feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - """ Testing feedback not set""" - feedback.feedback.feedback_choice = feedback.feedback.FEEDBACK_NOT_SET + """ Testing Gripper commands """ + gripper_feedback = RobotCommandFeedback().command.synchronized_feedback.gripper_command_feedback - self.assertEqual(self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), None) + feedback.command.synchronized_feedback.arm_command_feedback_is_set = False + feedback.command.synchronized_feedback.mobility_command_feedback_is_set = False + feedback.command.synchronized_feedback.gripper_command_feedback_is_set = True - """ Testing unknown command """ - feedback.feedback.feedback_choice = FEEDBACK_INVALID - self.assertEqual( - self.spot_ros2._process_synchronized_mobility_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_COMMAND_OVERRIDDEN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - def test_process_synchronized_gripper_command_feedback(self) -> None: - """Testing Gripper commands""" - feedback = GripperCommandFeedback() - - feedback.status.value = feedback.status.STATUS_COMMAND_OVERRIDDEN - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) - - feedback.status.value = feedback.status.STATUS_COMMAND_TIMED_OUT - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_COMMAND_TIMED_OUT + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_ROBOT_FROZEN - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_ROBOT_FROZEN + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_INCOMPATIBLE_HARDWARE - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.FAILED) + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_INCOMPATIBLE_HARDWARE + ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) - feedback.status.value = feedback.status.STATUS_PROCESSING + feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( + gripper_feedback.status.STATUS_PROCESSING + ) """ Testing Claw Gripper feedback """ - feedback.command.command_choice = feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET - feedback.command.claw_gripper_feedback.status.value = ( - feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = ( + gripper_feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET ) - self.assertEqual( - self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.claw_gripper_feedback.status.value = ( - feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN - ) - self.assertEqual( - self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN ) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - feedback.command.claw_gripper_feedback.status.value = ( - feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) - feedback.command.claw_gripper_feedback.status.value = ( - feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE + feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( + gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE ) - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), None) - - """ Testing feedback not set""" - feedback.command.command_choice = feedback.command.COMMAND_NOT_SET - - self.assertEqual(self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) """ Testing unknown gripper command """ - feedback.command.command_choice = FEEDBACK_INVALID - self.assertEqual( - self.spot_ros2._process_synchronized_gripper_command_feedback(feedback), GoalResponse.IN_PROGRESS - ) - - def test_robot_command_goal_complete(self) -> None: - self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) - - feedback = RobotCommandFeedback() - - feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET - - """ Testing Synchronized Feedback Command """ - - feedback.command.command_choice = feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET + feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = FEEDBACK_INVALID + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) """ Testing unknown robot command type """ feedback.command.command_choice = FEEDBACK_INVALID From 1ab35561126571652cc5a39531795000382590e8 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Mon, 23 Oct 2023 11:54:59 -0600 Subject: [PATCH 10/19] fix imports --- spot_driver/test/test_spot_driver.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index c0946ab6c..ac4647f48 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -3,9 +3,7 @@ import bdai_ros2_wrappers.scope as ros_scope import rclpy -from bosdyn_msgs.msg import ( - RobotCommandFeedback, -) +from bosdyn_msgs.msg import RobotCommandFeedback from std_srvs.srv import Trigger import spot_driver.spot_ros2 @@ -14,8 +12,6 @@ Dock, ) -FEEDBACK_INVALID = -128 - def spin_thread(executor: MultiThreadedExecutor) -> None: if executor is not None: try: From 1c20fe21be4eb35c48b375028bde52b24595e76f Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Mon, 23 Oct 2023 12:35:45 -0600 Subject: [PATCH 11/19] fix rebase mistakes --- spot_driver/test/test_spot_driver.py | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index ac4647f48..8922274c0 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -12,23 +12,6 @@ Dock, ) -def spin_thread(executor: MultiThreadedExecutor) -> None: - if executor is not None: - try: - executor.spin() - except (ExternalShutdownException, KeyboardInterrupt): - pass - - -def call_trigger_client( - client: rclpy.node.Client, executor: MultiThreadedExecutor, request: Any = Trigger.Request() -) -> spot_driver.spot_ros2.Response: - req = request - future = client.call_async(req) - executor.spin_until_future_complete(future) - resp = future.result() - return resp - class SpotDriverTest(unittest.TestCase): def setUp(self) -> None: From 6c5f74e8aa2bb838d40932939bdf13371657e167 Mon Sep 17 00:00:00 2001 From: Anthony Baker <141346257+abaker-bdai@users.noreply.github.com> Date: Tue, 7 Nov 2023 09:08:12 -0700 Subject: [PATCH 12/19] [PN-68] Add missing failure conditions and update unit test (#179) Co-authored-by: jbarry-bdai <116907872+jbarry-bdai@users.noreply.github.com> --- spot_driver/spot_driver/spot_ros2.py | 11 +++++++++++ spot_driver/test/test_spot_driver.py | 14 +++++--------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 59c95a25c..01ef03ee1 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1528,17 +1528,28 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac choice = fb.feedback_choice if choice == fb.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: + if ( + fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED + or fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED + ): + return GoalResponse.FAILED if fb.arm_cartesian_feedback.status.value != fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE: return GoalResponse.IN_PROGRESS elif choice == fb.FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET: + if fb.arm_joint_move_feedback.status.value == fb.arm_joint_move_feedback.status.STATUS_STALLED: + return GoalResponse.FAILED if fb.arm_joint_move_feedback.status.value != fb.arm_joint_move_feedback.status.STATUS_COMPLETE: return GoalResponse.IN_PROGRESS elif choice == fb.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET: + if fb.named_arm_position_feedback.status.value == fb.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM: + return GoalResponse.FAILED if fb.named_arm_position_feedback.status.value != fb.named_arm_position_feedback.status.STATUS_COMPLETE: return GoalResponse.IN_PROGRESS elif choice == fb.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET: self.get_logger().warn("WARNING: ArmVelocityCommand provides no feedback") elif choice == fb.FEEDBACK_ARM_GAZE_FEEDBACK_SET: + if fb.arm_gaze_feedback.status.value == fb.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED: + return GoalResponse.FAILED if fb.arm_gaze_feedback.status.value != fb.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE: return GoalResponse.IN_PROGRESS elif choice == fb.FEEDBACK_ARM_STOP_FEEDBACK_SET: diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 8922274c0..37ef8df62 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -266,16 +266,15 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # should these next two test cases be failures? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing arm joint move feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -297,11 +296,10 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing named arm position feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -323,11 +321,10 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing arm velocity feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -356,11 +353,10 @@ def test_robot_command_goal_complete(self) -> None: ) self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) - # Should this test case be a Failure? feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) """ Testing arm stop feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( From 81a52b94ea47e1bd27b86b04cf999eb9791feb9d Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Tue, 7 Nov 2023 09:12:20 -0700 Subject: [PATCH 13/19] pre-commit --- spot_driver/spot_driver/spot_ros2.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 01ef03ee1..08d8770ab 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1529,8 +1529,8 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac if choice == fb.FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET: if ( - fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED - or fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED + fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED + or fb.arm_cartesian_feedback.status.value == fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ): return GoalResponse.FAILED if fb.arm_cartesian_feedback.status.value != fb.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE: @@ -1541,7 +1541,10 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac if fb.arm_joint_move_feedback.status.value != fb.arm_joint_move_feedback.status.STATUS_COMPLETE: return GoalResponse.IN_PROGRESS elif choice == fb.FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET: - if fb.named_arm_position_feedback.status.value == fb.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM: + if ( + fb.named_arm_position_feedback.status.value + == fb.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM + ): return GoalResponse.FAILED if fb.named_arm_position_feedback.status.value != fb.named_arm_position_feedback.status.STATUS_COMPLETE: return GoalResponse.IN_PROGRESS From 073001ed626e3d8cc17a423295d55431356cf185 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 9 Nov 2023 09:11:50 -0700 Subject: [PATCH 14/19] Now returning GoalResponse.SUCCESS --- spot_driver/spot_driver/spot_ros2.py | 32 +++++++++++++++------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 08d8770ab..89eebc349 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1515,7 +1515,7 @@ def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) return GoalResponse.FAILED return GoalResponse.IN_PROGRESS - def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedback) -> Optional[GoalResponse]: + def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedback) -> GoalResponse: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1565,11 +1565,9 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac else: self.get_logger().error("ERROR: unknown arm command type") return GoalResponse.IN_PROGRESS - return None + return GoalResponse.SUCCESS - def _process_synchronized_mobility_command_feedback( - self, feedback: MobilityCommandFeedback - ) -> Optional[GoalResponse]: + def _process_synchronized_mobility_command_feedback(self, feedback: MobilityCommandFeedback) -> GoalResponse: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1608,11 +1606,9 @@ def _process_synchronized_mobility_command_feedback( else: self.get_logger().error("ERROR: unknown mobility command type") return GoalResponse.IN_PROGRESS - return None + return GoalResponse.SUCCESS - def _process_synchronized_gripper_command_feedback( - self, feedback: GripperCommandFeedback - ) -> Optional[GoalResponse]: + def _process_synchronized_gripper_command_feedback(self, feedback: GripperCommandFeedback) -> GoalResponse: if ( feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT @@ -1633,11 +1629,17 @@ def _process_synchronized_gripper_command_feedback( ): self.get_logger().error("ERROR: claw grippper status unknown") return GoalResponse.IN_PROGRESS - # else: STATUS_AT_GOAL or STATUS_APPLYING_FORCE + if ( + feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL + or feedback.command.claw_gripper_feedback.status.value + == feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE + ): + return GoalResponse.SUCCESS else: self.get_logger().error("ERROR: unknown gripper command type") return GoalResponse.IN_PROGRESS - return None + return GoalResponse.SUCCESS def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalResponse: if feedback is None: @@ -1664,22 +1666,22 @@ def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalRe if sync_feedback.arm_command_feedback_is_set is True: arm_feedback = sync_feedback.arm_command_feedback response = self._process_synchronized_arm_command_feedback(arm_feedback) - if response is not None: + if response is not GoalResponse.SUCCESS: return response if sync_feedback.mobility_command_feedback_is_set is True: mob_feedback = sync_feedback.mobility_command_feedback response = self._process_synchronized_mobility_command_feedback(mob_feedback) - if response is not None: + if response is not GoalResponse.SUCCESS: return response if sync_feedback.gripper_command_feedback_is_set is True: grip_feedback = sync_feedback.gripper_command_feedback response = self._process_synchronized_gripper_command_feedback(grip_feedback) - if response is not None: + if response is not GoalResponse.SUCCESS: return response - return GoalResponse.SUCCESS + return response else: self.get_logger().error("ERROR: unknown robot command type") return GoalResponse.IN_PROGRESS From cb0e9c8083a60b916774cf5ad0abe6eb52c4ec32 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 9 Nov 2023 09:12:56 -0700 Subject: [PATCH 15/19] Added test annotations for better identification --- spot_driver/test/test_spot_driver.py | 506 ++++++++++++++++++++++----- 1 file changed, 419 insertions(+), 87 deletions(-) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 37ef8df62..bd86e49ec 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -72,7 +72,7 @@ def test_wrapped_commands(self) -> None: def test_robot_command_goal_complete(self) -> None: FEEDBACK_INVALID = -128 - self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS) + self.assertEqual(self.spot_ros2._robot_command_goal_complete(None), GoalResponse.IN_PROGRESS, "Empty Command") feedback = RobotCommandFeedback() @@ -82,7 +82,11 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.command_choice = feedback.command.COMMAND_FULL_BODY_FEEDBACK_SET feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_UNKNOWN - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "COMMAND_FULL_BODY_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.full_body_feedback.status.value = fullbody_feedback.status.STATUS_PROCESSING @@ -90,13 +94,17 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.feedback_choice = ( fullbody_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS, "FEEDBACK_STOP_FEEDBACK_SET" + ) """ Testing FREEZE_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( fullbody_feedback.feedback.FEEDBACK_FREEZE_FEEDBACK_SET ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS, "FEEDBACK_FREEZE_FEEDBACK_SET" + ) """ Testing SELFRIGHT_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( @@ -105,17 +113,29 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( fullbody_feedback.feedback.selfright_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_SELFRIGHT_FEEDBACK_SET | STATUS_COMPLETED", + ) feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( fullbody_feedback.feedback.selfright_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SELFRIGHT_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.full_body_feedback.feedback.selfright_feedback.status.value = ( fullbody_feedback.feedback.selfright_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SELFRIGHT_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) """ Testing SAFE_POWER_OFF_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( @@ -124,17 +144,29 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_POWERED_OFF ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET | STATUS_POWERED_OFF", + ) feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.full_body_feedback.feedback.safe_power_off_feedback.status.value = ( fullbody_feedback.feedback.safe_power_off_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SAFE_POWER_OFF_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) """ Testing BATTERY_CHANGE_POSE_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( @@ -143,22 +175,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET | STATUS_COMPLETED", + ) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.full_body_feedback.feedback.battery_change_pose_feedback.status.value = ( fullbody_feedback.feedback.battery_change_pose_feedback.status.STATUS_FAILED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET | STATUS_FAILED", + ) """ Testing PAYLOAD_ESTIMATION_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( @@ -167,27 +215,47 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_COMPLETED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET | STATUS_COMPLETED", + ) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_SMALL_MASS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET | STATUS_SMALL_MASS", + ) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET | STATUS_ERROR", + ) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.full_body_feedback.feedback.payload_estimation_feedback.status.value = ( fullbody_feedback.feedback.payload_estimation_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) """ Testing CONSTRAINED_MANIPULATION_FEEDBACK_SET """ feedback.command.full_body_feedback.feedback.feedback_choice = ( @@ -196,22 +264,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET | STATUS_RUNNING", + ) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_GRASP_IS_LOST ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET | STATUS_GRASP_IS_LOST", + ) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_ARM_IS_STUCK ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET | STATUS_ARM_IS_STUCK", + ) feedback.command.full_body_feedback.feedback.constrained_manipulation_feedback.status.value = ( fullbody_feedback.feedback.constrained_manipulation_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET | STATUS_UNKNOWN", + ) """ Testing Synchronized Feedback Command """ arm_command_feedback = feedback.command.synchronized_feedback.arm_command_feedback @@ -225,22 +309,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( arm_command_feedback.status.STATUS_COMMAND_OVERRIDDEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "Arm_command | COMMAND_SYNCHRONIZED_FEEDBACK_SET | STATUS_COMMAND_OVERRIDDEN", + ) feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( arm_command_feedback.status.STATUS_COMMAND_TIMED_OUT ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "Arm_command | COMMAND_SYNCHRONIZED_FEEDBACK_SET | STATUS_COMMAND_TIMED_OUT", + ) feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( arm_command_feedback.status.STATUS_ROBOT_FROZEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "Arm_command | COMMAND_SYNCHRONIZED_FEEDBACK_SET | STATUS_ROBOT_FROZEN", + ) feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( arm_command_feedback.status.STATUS_INCOMPATIBLE_HARDWARE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "Arm_command | COMMAND_SYNCHRONIZED_FEEDBACK_SET | STATUS_INCOMPATIBLE_HARDWARE", + ) feedback.command.synchronized_feedback.arm_command_feedback.status.value = ( arm_command_feedback.status.STATUS_PROCESSING @@ -254,27 +354,47 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET | STATUS_TRAJECTORY_COMPLETE", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_CANCELLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET | STATUS_TRAJECTORY_CANCELLED", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_cartesian_feedback.status.value = ( arm_command_feedback.feedback.arm_cartesian_feedback.status.STATUS_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_CARTESIAN_FEEDBACK_SET | STATUS_TRAJECTORY_STALLED", + ) """ Testing arm joint move feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -284,22 +404,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET | STATUS_COMPLETE", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_joint_move_feedback.status.value = ( arm_command_feedback.feedback.arm_joint_move_feedback.status.STATUS_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET | STATUS_STALLED", + ) """ Testing named arm position feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -309,29 +445,49 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET | STATUS_COMPLETE", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.named_arm_position_feedback.status.value = ( arm_command_feedback.feedback.named_arm_position_feedback.status.STATUS_STALLED_HOLDING_ITEM ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_NAMED_ARM_POSITION_FEEDBACK_SET | STATUS_STALLED_HOLDING_ITEM", + ) """ Testing arm velocity feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( arm_command_feedback.feedback.FEEDBACK_ARM_VELOCITY_FEEDBACK_SET ) # Arm velocity commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_VELOCITY_FEEDBACK_SET", + ) """ Testing arm gaze feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -341,29 +497,49 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_GAZE_FEEDBACK_SET | STATUS_TRAJECTORY_COMPLETE", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_GAZE_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_GAZE_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_gaze_feedback.status.value = ( arm_command_feedback.feedback.arm_gaze_feedback.status.STATUS_TOOL_TRAJECTORY_STALLED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_GAZE_FEEDBACK_SET | STATUS_TOOL_TRAJECTORY_STALLED", + ) """ Testing arm stop feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( arm_command_feedback.feedback.FEEDBACK_ARM_STOP_FEEDBACK_SET ) # Arm stop commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_STOP_FEEDBACK_SET", + ) """ Testing arm drag feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( @@ -373,41 +549,69 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_DRAGGING ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_DRAG_FEEDBACK_SET | STATUS_DRAGGING", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_DRAG_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_GRASP_FAILED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_DRAG_FEEDBACK_SET | STATUS_GRASP_FAILED", + ) feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_drag_feedback.status.value = ( arm_command_feedback.feedback.arm_drag_feedback.status.STATUS_OTHER_FAILURE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_DRAG_FEEDBACK_SET | STATUS_OTHER_FAILURE", + ) """ Testing arm drag feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET ) # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET", + ) """ Testing arm drag feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET ) # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET", + ) """ Testing unknown arm command """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID # Arm impedance commands do not provide feedback therefore we should get a success - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "Arm Command: FEEDBACK_INVALID", + ) """ Testing mobility commands """ mobility_feedback = RobotCommandFeedback().command.synchronized_feedback.mobility_command_feedback @@ -419,22 +623,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( mobility_feedback.status.STATUS_COMMAND_OVERRIDDEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "MOBILITY | COMMAND_SYNCHRONIZED_FEEDBACK | STATUS_COMMAND_OVERRIDDEN", + ) feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( mobility_feedback.status.STATUS_COMMAND_TIMED_OUT ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "MOBILITY | COMMAND_SYNCHRONIZED_FEEDBACK | | STATUS_COMMAND_TIMED_OUT", + ) feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( mobility_feedback.status.STATUS_ROBOT_FROZEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "MOBILITY | COMMAND_SYNCHRONIZED_FEEDBACK | | STATUS_ROBOT_FROZEN", + ) feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( mobility_feedback.status.STATUS_INCOMPATIBLE_HARDWARE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "MOBILITY | COMMAND_SYNCHRONIZED_FEEDBACK | | STATUS_INCOMPATIBLE_HARDWARE", + ) feedback.command.synchronized_feedback.mobility_command_feedback.status.value = ( mobility_feedback.status.STATUS_PROCESSING @@ -448,29 +668,49 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET | STATUS_AT_GOAL", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_NEAR_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET | STATUS_NEAR_GOAL", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.se2_trajectory_feedback.status.value = ( mobility_feedback.feedback.se2_trajectory_feedback.status.STATUS_GOING_TO_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SE2_TRAJECTORY_FEEDBACK_SET | STATUS_GOING_TO_GOAL", + ) """ Testing se2 velocity feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( mobility_feedback.feedback.FEEDBACK_SE2_VELOCITY_FEEDBACK_SET ) # Planar velocity commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_SE2_VELOCITY_FEEDBACK_SET", + ) """ Testing sit feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( @@ -480,17 +720,29 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( mobility_feedback.feedback.sit_feedback.status.STATUS_IS_SITTING ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_SIT_FEEDBACK_SET | STATUS_IS_SITTING", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( mobility_feedback.feedback.sit_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SIT_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.sit_feedback.status.value = ( mobility_feedback.feedback.sit_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_SIT_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) """ Testing stand feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( @@ -500,17 +752,29 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( mobility_feedback.feedback.stand_feedback.status.STATUS_IS_STANDING ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_STAND_FEEDBACK_SET | STATUS_IS_STANDING", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( mobility_feedback.feedback.stand_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_STAND_FEEDBACK_SET | STATUS_UNKNOWN", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stand_feedback.status.value = ( mobility_feedback.feedback.stand_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_STAND_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) """ Testing stance feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( @@ -520,36 +784,58 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( mobility_feedback.feedback.stance_feedback.status.STATUS_TOO_FAR_AWAY ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_STANCE_FEEDBACK_SET | STATUS_TOO_FAR_AWAY", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( mobility_feedback.feedback.stance_feedback.status.STATUS_STANCED ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_STANCE_FEEDBACK_SET | STATUS_STANCED", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( mobility_feedback.feedback.stance_feedback.status.STATUS_GOING_TO_STANCE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_STANCE_FEEDBACK_SET | STATUS_GOING_TO_STANCE", + ) feedback.command.synchronized_feedback.mobility_command_feedback.feedback.stance_feedback.status.value = ( mobility_feedback.feedback.stance_feedback.status.STATUS_UNKNOWN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_STANCE_FEEDBACK_SET | STATUS_UNKNOWN", + ) """ Testing stop feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( mobility_feedback.feedback.FEEDBACK_STOP_FEEDBACK_SET ) # Stop commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS, "FEEDBACK_STOP_FEEDBACK_SET" + ) """ Testing stop feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( mobility_feedback.feedback.FEEDBACK_FOLLOW_ARM_FEEDBACK_SET ) # follow arm commands provide no feedback, therefore expect SUCCESS - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "FEEDBACK_FOLLOW_ARM_FEEDBACK_SET", + ) """ Testing stop feedback """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = ( @@ -557,11 +843,17 @@ def test_robot_command_goal_complete(self) -> None: ) # mobility command feedback is not set, this could be caused by a command that finishes and resets the feedback status. # because of this case, it will return success as long as no other synchronous commands are run afterwards. - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS, "MOBILITY | FEEDBACK_NOT_SET" + ) """ Testing unknown command """ feedback.command.synchronized_feedback.mobility_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "MOBILITY| FEEDBACK_INVALID", + ) """ Testing Gripper commands """ gripper_feedback = RobotCommandFeedback().command.synchronized_feedback.gripper_command_feedback @@ -573,22 +865,38 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( gripper_feedback.status.STATUS_COMMAND_OVERRIDDEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "GRIPPER | COMMAND_SYNCHRONIZED_FEEDBACK | STATUS_COMMAND_OVERRIDDEN", + ) feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( gripper_feedback.status.STATUS_COMMAND_TIMED_OUT ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "GRIPPER | COMMAND_SYNCHRONIZED_FEEDBACK | STATUS_COMMAND_TIMED_OUT", + ) feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( gripper_feedback.status.STATUS_ROBOT_FROZEN ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "GRIPPER | COMMAND_SYNCHRONIZED_FEEDBACK | STATUS_ROBOT_FROZEN", + ) feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( gripper_feedback.status.STATUS_INCOMPATIBLE_HARDWARE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.FAILED) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "GRIPPER | COMMAND_SYNCHRONIZED_FEEDBACK | STATUS_INCOMPATIBLE_HARDWARE", + ) feedback.command.synchronized_feedback.gripper_command_feedback.status.value = ( gripper_feedback.status.STATUS_PROCESSING @@ -601,30 +909,54 @@ def test_robot_command_goal_complete(self) -> None: feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( gripper_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( - gripper_feedback.command.claw_gripper_feedback.status.STATUS_UNKNOWN + gripper_feedback.command.claw_gripper_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | STATUS_IN_PROGRESS", ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( gripper_feedback.command.claw_gripper_feedback.status.STATUS_AT_GOAL ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | STATUS_AT_GOAL", + ) feedback.command.synchronized_feedback.gripper_command_feedback.command.claw_gripper_feedback.status.value = ( gripper_feedback.command.claw_gripper_feedback.status.STATUS_APPLYING_FORCE ) - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.SUCCESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | STATUS_APPLYING_FORCE", + ) """ Testing unknown gripper command """ feedback.command.synchronized_feedback.gripper_command_feedback.command.command_choice = FEEDBACK_INVALID - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | FEEDBACK_INVALID", + ) """ Testing unknown robot command type """ feedback.command.command_choice = FEEDBACK_INVALID - self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "COMMAND_CLAW_GRIPPER_FEEDBACK_SET | FEEDBACK_INVALID", + ) if __name__ == "__main__": From 9ed93ad94c2e160435b06c97dd15a9a7582323b3 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 9 Nov 2023 10:33:13 -0700 Subject: [PATCH 16/19] fixed logic --- spot_driver/spot_driver/spot_ros2.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 89eebc349..ed72e2744 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1558,7 +1558,9 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac elif choice == fb.FEEDBACK_ARM_STOP_FEEDBACK_SET: self.get_logger().warn("WARNING: Stop command provides no feedback") elif choice == fb.FEEDBACK_ARM_DRAG_FEEDBACK_SET: - if fb.arm_drag_feedback.status.value != fb.arm_drag_feedback.status.STATUS_DRAGGING: + if fb.arm_drag_feedback.status.value == fb.arm_drag_feedback.status.STATUS_DRAGGING: + return GoalResponse.IN_PROGRESS + else: return GoalResponse.FAILED elif choice == fb.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") From 3910fe216a0ce54902921d4b640021a9754761d1 Mon Sep 17 00:00:00 2001 From: Anthony Baker <141346257+abaker-bdai@users.noreply.github.com> Date: Tue, 5 Dec 2023 09:18:33 -0700 Subject: [PATCH 17/19] Add logic for Arm Impedance feedback statuses (#204) --- spot_driver/spot_driver/spot_ros2.py | 8 +++++- spot_driver/test/test_spot_driver.py | 37 ++++++++++++++++++++-------- 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index ed72e2744..08d7e925c 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1563,7 +1563,13 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac else: return GoalResponse.FAILED elif choice == fb.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET: - self.get_logger().warn("WARNING: ArmImpedanceCommand provides no feedback") + if ( + fb.arm_impedance_feedback.status.value == fb.arm_impedance_feedback.status.STATUS_TRAJECTORY_STALLED + or fb.arm_impedance_feedback.status.value == fb.arm_impedance_feedback.status.STATUS_UNKNOWN + ): + return GoalResponse.FAILED + if fb.arm_impedance_feedback.status.value != fb.arm_impedance_feedback.status.STATUS_TRAJECTORY_COMPLETE: + return GoalResponse.IN_PROGRESS else: self.get_logger().error("ERROR: unknown arm command type") return GoalResponse.IN_PROGRESS diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index bd86e49ec..011c5b5e7 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -582,31 +582,48 @@ def test_robot_command_goal_complete(self) -> None: "FEEDBACK_ARM_DRAG_FEEDBACK_SET | STATUS_OTHER_FAILURE", ) - """ Testing arm drag feedback """ + """ Testing arm impedance feedback """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET ) - # Arm impedance commands do not provide feedback therefore we should get a success + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_impedance_feedback.status.value = ( + arm_command_feedback.feedback.arm_impedance_feedback.status.STATUS_UNKNOWN + ) self.assertEqual( self.spot_ros2._robot_command_goal_complete(feedback), - GoalResponse.SUCCESS, - "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET", + GoalResponse.FAILED, + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET | STATUS_UNKNOWN", ) - """ Testing arm drag feedback """ - feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = ( - arm_command_feedback.feedback.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_impedance_feedback.status.value = ( + arm_command_feedback.feedback.arm_impedance_feedback.status.STATUS_TRAJECTORY_COMPLETE ) - # Arm impedance commands do not provide feedback therefore we should get a success self.assertEqual( self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.SUCCESS, - "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET", + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET | STATUS_TRAJECTORY_COMPLETE", + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_impedance_feedback.status.value = ( + arm_command_feedback.feedback.arm_impedance_feedback.status.STATUS_IN_PROGRESS + ) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.IN_PROGRESS, + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET | STATUS_IN_PROGRESS", + ) + + feedback.command.synchronized_feedback.arm_command_feedback.feedback.arm_impedance_feedback.status.value = ( + arm_command_feedback.feedback.arm_impedance_feedback.status.STATUS_TRAJECTORY_STALLED + ) + self.assertEqual( + self.spot_ros2._robot_command_goal_complete(feedback), + GoalResponse.FAILED, + "FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET | STATUS_TRAJECTORY_STALLED", ) """ Testing unknown arm command """ feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = FEEDBACK_INVALID - # Arm impedance commands do not provide feedback therefore we should get a success self.assertEqual( self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.IN_PROGRESS, From 7f1ff6622a99772db837b3e1b970da3d30f1f9b7 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 14 Dec 2023 13:51:22 +0000 Subject: [PATCH 18/19] robot command feedback better documented --- spot_driver/spot_driver/spot_ros2.py | 60 +++++++++++++++++----------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 08d7e925c..d8697a4d0 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -46,6 +46,7 @@ ManipulatorState, MobilityCommandFeedback, RobotCommandFeedback, + RobotCommandFeedbackStatusStatus, ) from geometry_msgs.msg import ( Pose, @@ -1470,10 +1471,33 @@ def handle_max_vel(self, request: SetVelocity.Request, response: SetVelocity.Res response.message = f"Error: {e}" return response - def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) -> GoalResponse: - if feedback.status.value != feedback.status.STATUS_PROCESSING: + def _process_feedback_status(self, status: int) -> Optional[GoalResponse]: + if status == RobotCommandFeedbackStatusStatus.STATUS_UNKNOWN: return GoalResponse.IN_PROGRESS + if status == RobotCommandFeedbackStatusStatus.STATUS_COMMAND_OVERRIDDEN: + self.get_logger().warn("Command has been overwritten") + return GoalResponse.FAILED + + if status == RobotCommandFeedbackStatusStatus.STATUS_COMMAND_TIMED_OUT: + self.get_logger().warn("Command has timed out") + return GoalResponse.FAILED + + if status == RobotCommandFeedbackStatusStatus.STATUS_ROBOT_FROZEN: + self.get_logger().warn("Robot is in unsafe state. Will only respond to safe commands") + return GoalResponse.FAILED + + if status == RobotCommandFeedbackStatusStatus.STATUS_INCOMPATIBLE_HARDWARE: + self.get_logger().warn("Command is incompatible with current hardware") + return GoalResponse.FAILED + + return None + + def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) -> GoalResponse: + maybe_goal_response = self._process_feedback_status(feedback.status.value) + if maybe_goal_response is not None: + return maybe_goal_response + fb = feedback.feedback choice = fb.feedback_choice @@ -1516,13 +1540,9 @@ def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) return GoalResponse.IN_PROGRESS def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedback) -> GoalResponse: - if ( - feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN - or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT - or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN - or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ): - return GoalResponse.FAILED + maybe_goal_response = self._process_feedback_status(feedback.status.value) + if maybe_goal_response is not None: + return maybe_goal_response fb = feedback.feedback choice = fb.feedback_choice @@ -1576,13 +1596,9 @@ def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedbac return GoalResponse.SUCCESS def _process_synchronized_mobility_command_feedback(self, feedback: MobilityCommandFeedback) -> GoalResponse: - if ( - feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN - or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT - or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN - or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ): - return GoalResponse.FAILED + maybe_goal_response = self._process_feedback_status(feedback.status.value) + if maybe_goal_response is not None: + return maybe_goal_response fb = feedback.feedback choice = fb.feedback_choice @@ -1617,13 +1633,9 @@ def _process_synchronized_mobility_command_feedback(self, feedback: MobilityComm return GoalResponse.SUCCESS def _process_synchronized_gripper_command_feedback(self, feedback: GripperCommandFeedback) -> GoalResponse: - if ( - feedback.status.value == feedback.status.STATUS_COMMAND_OVERRIDDEN - or feedback.status.value == feedback.status.STATUS_COMMAND_TIMED_OUT - or feedback.status.value == feedback.status.STATUS_ROBOT_FROZEN - or feedback.status.value == feedback.status.STATUS_INCOMPATIBLE_HARDWARE - ): - return GoalResponse.FAILED + maybe_goal_response = self._process_feedback_status(feedback.status.value) + if maybe_goal_response is not None: + return maybe_goal_response if feedback.command.command_choice == feedback.command.COMMAND_CLAW_GRIPPER_FEEDBACK_SET: if ( @@ -1665,7 +1677,7 @@ def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalRe # So if any one of the sub-commands is still in progress, it short-circuits out as # IN_PROGRESS. And if it makes it to the bottom of the function, then all components # must be satisfied, and it returns SUCCESS. - # One corner if choice == to know about is that the commands that don't provide feedback, such + # One corner case to know about is that the commands that don't provide feedback, such # as a velocity command will return SUCCESS. This allows you to use them more effectively # with other commands. For example if you wanted to move the arm with some velocity # while the mobility is going to some SE2 trajectory then that will work. From 9e8ff97bb182fb357f610736eced118163ce8875 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Thu, 14 Dec 2023 13:59:22 +0000 Subject: [PATCH 19/19] add comment --- spot_driver/spot_driver/spot_ros2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index d8697a4d0..aaa6b827a 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -1491,6 +1491,8 @@ def _process_feedback_status(self, status: int) -> Optional[GoalResponse]: self.get_logger().warn("Command is incompatible with current hardware") return GoalResponse.FAILED + # if status == RobotCommandFeedbackStatusStatus.STATUS_PROCESSING, + # return None to continue processing the command feedback return None def _process_full_body_command_feedback(self, feedback: FullBodyCommandFeedback) -> GoalResponse: