diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py
index aa4020660..aaa6b827a 100644
--- a/spot_driver/spot_driver/spot_ros2.py
+++ b/spot_driver/spot_driver/spot_ros2.py
@@ -39,9 +39,14 @@
 from bosdyn.client import math_helpers
 from bosdyn.client.exceptions import InternalServerError
 from bosdyn_msgs.msg import (
+    ArmCommandFeedback,
+    FullBodyCommandFeedback,
+    GripperCommandFeedback,
     ManipulationApiFeedbackResponse,
     ManipulatorState,
+    MobilityCommandFeedback,
     RobotCommandFeedback,
+    RobotCommandFeedbackStatusStatus,
 )
 from geometry_msgs.msg import (
     Pose,
@@ -1466,92 +1471,209 @@ 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_feedback_status(self, status: int) -> Optional[GoalResponse]:
+        if status == RobotCommandFeedbackStatusStatus.STATUS_UNKNOWN:
             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:
+        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
+
+        # 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:
+        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
+
+        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
-            elif (
-                full_body_feedback.feedback.feedback_choice == full_body_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
-            elif (
-                full_body_feedback.feedback.feedback_choice
-                == full_body_feedback.feedback.FEEDBACK_SELFRIGHT_FEEDBACK_SET
-            ):
-                if (
-                    full_body_feedback.feedback.selfright_feedback.status.value
-                    == full_body_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
-            ):
-                if (
-                    full_body_feedback.feedback.safe_power_off_feedback.status.value
-                    == full_body_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
+
+        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
+            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
             ):
-                if (
-                    full_body_feedback.feedback.battery_change_pose_feedback.status.value
-                    == full_body_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
-                ):
-                    return GoalResponse.FAILED
                 return GoalResponse.IN_PROGRESS
-            elif (
-                full_body_feedback.feedback.feedback_choice
-                == full_body_feedback.feedback.FEEDBACK_PAYLOAD_ESTIMATION_FEEDBACK_SET
+            return GoalResponse.FAILED
+        return GoalResponse.IN_PROGRESS
+
+    def _process_synchronized_arm_command_feedback(self, feedback: ArmCommandFeedback) -> 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
+
+        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
             ):
-                if (
-                    full_body_feedback.feedback.payload_estimation_feedback.status.value
-                    == full_body_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
-                ):
-                    return GoalResponse.SUCCESS
-                if (
-                    full_body_feedback.feedback.payload_estimation_feedback.status.value
-                    == full_body_feedback.feedback.payload_estimation_feedback.status.STATUS_ERROR
-                ):
-                    return GoalResponse.FAILED
+                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 (
-                full_body_feedback.feedback.feedback_choice
-                == full_body_feedback.feedback.FEEDBACK_CONSTRAINED_MANIPULATION_FEEDBACK_SET
+        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 (
-                    full_body_feedback.feedback.constrained_manipulation_feedback.status.value
-                    == full_body_feedback.feedback.constrained_manipulation_feedback.status.STATUS_RUNNING
-                ):
-                    return GoalResponse.IN_PROGRESS
                 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:
+            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.IN_PROGRESS
             else:
+                return GoalResponse.FAILED
+        elif choice == fb.FEEDBACK_ARM_IMPEDANCE_FEEDBACK_SET:
+            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
+        return GoalResponse.SUCCESS
+
+    def _process_synchronized_mobility_command_feedback(self, feedback: MobilityCommandFeedback) -> 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
+
+        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 GoalResponse.SUCCESS
+
+    def _process_synchronized_gripper_command_feedback(self, feedback: GripperCommandFeedback) -> GoalResponse:
+        maybe_goal_response = self._process_feedback_status(feedback.status.value)
+        if maybe_goal_response is not None:
+            return maybe_goal_response
 
-        elif feedback.command.command_choice == feedback.command.COMMAND_SYNCHRONIZED_FEEDBACK_SET:
+        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
+            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 GoalResponse.SUCCESS
+
+    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
+
+        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
@@ -1565,146 +1687,23 @@ def _robot_command_goal_complete(self, feedback: RobotCommandFeedback) -> GoalRe
             sync_feedback = feedback.command.synchronized_feedback
             if sync_feedback.arm_command_feedback_is_set is True:
                 arm_feedback = sync_feedback.arm_command_feedback
-                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
-                ):
-                    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
-                ):
-                    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
-                ):
-                    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
+                response = self._process_synchronized_arm_command_feedback(arm_feedback)
+                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
-                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
-                ):
-                    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")
-                    return GoalResponse.IN_PROGRESS
+                response = self._process_synchronized_mobility_command_feedback(mob_feedback)
+                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
-                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
-                ):
-                    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")
-                    return GoalResponse.IN_PROGRESS
-
-            return GoalResponse.SUCCESS
+                response = self._process_synchronized_gripper_command_feedback(grip_feedback)
+                if response is not GoalResponse.SUCCESS:
+                    return response
 
+            return response
         else:
             self.get_logger().error("ERROR: unknown robot 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 8922274c0..011c5b5e7 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,28 +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",
+        )
 
-        # 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_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.IN_PROGRESS)
+        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 = (
@@ -285,23 +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",
+        )
 
-        # 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,
+            "FEEDBACK_ARM_JOINT_MOVE_FEEDBACK_SET | STATUS_STALLED",
+        )
 
         """ Testing named arm position feedback """
         feedback.command.synchronized_feedback.arm_command_feedback.feedback.feedback_choice = (
@@ -311,30 +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",
+        )
 
-        # 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,
+            "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 = (
@@ -344,30 +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",
+        )
 
-        # 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,
+            "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 = (
@@ -377,41 +549,86 @@ 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 """
+        """ 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
-        self.assertEqual(self.spot_ros2._robot_command_goal_complete(feedback), GoalResponse.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.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
+        )
+        self.assertEqual(
+            self.spot_ros2._robot_command_goal_complete(feedback),
+            GoalResponse.SUCCESS,
+            "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",
         )
-        # 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)
+        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
@@ -423,22 +640,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
@@ -452,29 +685,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 = (
@@ -484,17 +737,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 = (
@@ -504,17 +769,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 = (
@@ -524,36 +801,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 = (
@@ -561,11 +860,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
@@ -577,22 +882,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
@@ -605,30 +926,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__":