From e2df69549538ec3e32859e96c2356df9b36e5a95 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Sat, 23 Dec 2023 12:51:44 +0100
Subject: [PATCH] fix: fix 'set_joint_commands' service return values (#205)

This commit ensures that the 'set_join_commands' service doesn't crash
when the hand or arm commands are omitted.
---
 .../src/panda_gazebo/core/control_server.py   | 33 +++++++++++--------
 .../tests/manual/panda_control_server_test.py |  8 ++++-
 2 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py
index e9fd09c2..bb159445 100644
--- a/panda_gazebo/src/panda_gazebo/core/control_server.py
+++ b/panda_gazebo/src/panda_gazebo/core/control_server.py
@@ -1202,24 +1202,29 @@ def _set_joint_commands_cb(self, set_joint_commands_req):
                 )
 
         # Check if everything went OK and return response.
-        if (
-            arm_resp
-            and arm_resp.success
-            and (
-                not self._load_gripper or (self._load_gripper and gripper_resp.success)
-            )
-        ):
-            resp.success = True
-            resp.message = "Everything went OK"
-        elif self._load_gripper and gripper_resp and not gripper_resp.success:
+        arm_control_failed = (
+            arm_command_msg is not None
+            and arm_resp is not None
+            and not arm_resp.success
+        )
+        gripper_control_failed = (
+            self._load_gripper
+            and gripper_command_msg is not None
+            and gripper_resp is not None
+            and not gripper_resp.success
+        )
+        if arm_control_failed and gripper_control_failed:
             resp.success = False
-            resp.message = "Gripper control failed"
-        elif arm_resp and not arm_resp.success:
+            resp.message = "Joint control failed"
+        elif arm_control_failed:
             resp.success = False
             resp.message = "Arm control failed"
-        else:
+        elif gripper_control_failed:
             resp.success = False
-            resp.message = "Joint control failed"
+            resp.message = "Gripper control failed"
+        else:
+            resp.success = True
+            resp.message = "Everything went OK"
         return resp
 
     def _arm_set_joint_positions_cb(self, set_joint_positions_req):  # noqa: C901
diff --git a/panda_gazebo/tests/manual/panda_control_server_test.py b/panda_gazebo/tests/manual/panda_control_server_test.py
index 4c8c15b7..b3b57883 100644
--- a/panda_gazebo/tests/manual/panda_control_server_test.py
+++ b/panda_gazebo/tests/manual/panda_control_server_test.py
@@ -52,6 +52,10 @@
         "panda_joint1",
         "panda_joint2",
         "panda_joint3",
+        "panda_joint4",
+        "panda_joint5",
+        "panda_joint6",
+        "panda_joint7",
         "gripper_width",
         "gripper_max_effort",
     ]
@@ -74,7 +78,9 @@
     set_joint_commands_msg.control_type = "position"
     # set_joint_commands_msg.control_type = "effort"
     # set_joint_commands_msg.joint_commands = [1, 2, 3, 0.03]  # NOTE: Wrong input!
-    set_joint_commands_msg.joint_commands = [1, 2, 3, 0.03, 130]
+    # set_joint_commands_msg.joint_commands = [0.04, 10]
+    # set_joint_commands_msg.joint_commands = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+    set_joint_commands_msg.joint_commands = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.04, 10]
     set_joint_commands_msg.grasping = True
     resp = set_arm_joint_effort_srv.call(set_joint_commands_msg)
     print(resp.message)