Feature/string error codes

pymoveit2/gripper_command.py

@@ -14,6 +14,8 @@
 )
 from sensor_msgs.msg import JointState
 
+from pymoveit2.utils import enum_to_str
+
 
 class GripperCommand:
     """
@@ -289,7 +291,7 @@ def __response_callback_gripper_command(self, response):
     def __result_callback_gripper_command(self, res):
         if res.result().status != GoalStatus.STATUS_SUCCEEDED:
             self._node.get_logger().error(
-                f"Action '{self.__gripper_command_action_client._action_name}' was unsuccessful: {res.result().status}"
+                f"Action '{self.__gripper_command_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus, res.result().status)}"
             )
 
         self.__is_executing = False

pymoveit2/moveit2.py

@@ -41,6 +41,8 @@
 from std_msgs.msg import Header, String
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
+from pymoveit2.utils import enum_to_str
+
 
 class MoveIt2State(Enum):
     """
@@ -703,7 +705,7 @@ def get_trajectory(
                     return None
             else:
                 self._node.get_logger().warn(
-                    f"Planning failed! Error code: {res.error_code.val}."
+                    f"Planning failed! Error code: {enum_to_str(MoveItErrorCodes, res.error_code.val)}"
                 )
                 return None
 
@@ -713,7 +715,7 @@ def get_trajectory(
             return res.trajectory.joint_trajectory
         else:
             self._node.get_logger().warn(
-                f"Planning failed! Error code: {res.error_code.val}."
+                f"Planning failed! Error code: {enum_to_str(MoveItErrorCodes, res.error_code.val)}"
             )
             return None
 
@@ -1318,7 +1320,7 @@ def get_compute_ik_result(
             return res.solution.joint_state
         else:
             self._node.get_logger().warn(
-                f"IK computation failed! Error code: {res.error_code.val}."
+                f"IK computation failed! Error code: {enum_to_str(MoveItErrorCodes, res.error_code.val)}"
             )
             return None
 
@@ -2110,7 +2112,7 @@ def __result_callback_move_action(self, res):
         self.__execution_mutex.acquire()
         if res.result().status != GoalStatus.STATUS_SUCCEEDED:
             self._node.get_logger().warn(
-                f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}."
+                f"Action '{self.__move_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus,res.result().status)}."
             )
             self.motion_suceeded = False
         else:
@@ -2176,7 +2178,7 @@ def __result_callback_execute_trajectory(self, res):
         self.__execution_mutex.acquire()
         if res.result().status != GoalStatus.STATUS_SUCCEEDED:
             self._node.get_logger().warn(
-                f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}."
+                f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus,res.result().status)}."
             )
             self.motion_suceeded = False
         else:

pymoveit2/utils.py

@@ -0,0 +1,23 @@
+from typing import Type
+
+
+def enum_to_str(enum_class: Type, value: int) -> str:
+    """Converts a ROS2 enum value to its string name.
+
+    Args:
+        enum_class: The ROS2 message class containing the enum constants.
+        value: The integer value of the enum.
+
+    Returns:
+        str: The name of the enum constant, or the value with "UNKNOWN NAME" if not found.
+    """
+    mapping = {}
+    # Iterate over all attributes in the enum class
+    for attr_name in dir(enum_class):
+        # Consider only uppercase attributes (common convention for enums)
+        if attr_name.isupper():
+            attr_value = getattr(enum_class, attr_name)
+            # Check if the attribute is an integer (enum values are typically int)
+            if isinstance(attr_value, int):
+                mapping[attr_value] = attr_name
+    return mapping.get(value, f"{value} :UNKNOWN NAME")