Tool Controller fixed

niryo_one_tools/niryo_one_tools/tool_controller.py

@@ -162,41 +162,55 @@ def tool_on_goal(self, goal):
             self.node.get_logger().info("Tool controller - error : " + str(message))
             self.server.set_aborted(self.create_action_result(400, message))
 
-    def callback_change_tool(self, req):
+    def callback_change_tool(self, req, response):
         new_tool_id = req.value
 
         # Check if action == detach tool (id == 0)
         if new_tool_id == 0:
             self.ros_command_interface.ping_dxl_tool(0, 'No Dxl Tool')
             self.current_tool = None
-            self.publish_current_tool_id(None)
-            return self.create_response(200, "Tool has been detached")
+            self.publish_current_tool_id()
+            response.status = 200
+            response.message = "Tool has been detached"
+            return response
 
         # Check if tool id is null
         if new_tool_id is None:
-            return self.create_response(400, "No tool ID provided")
+            response.status = 400
+            response.message = "No tool ID provided"
+            return response
 
         if self.current_tool is not None:
             if self.current_tool.get_id() == new_tool_id:
-                return self.create_response(200, "This tool has already been selected")
+                response.status = 200
+                response.message = "This tool has already been selected"
+                return response
             if self.current_tool.is_active:
-                return self.create_response(400, "Current tool is still active, please retry later")
+                response.status = 400
+                response.message = "Current tool is still active, please retry later"
+                return response
 
         # look for new tool in available tools array
         for tool in self.available_tools:
             if new_tool_id == tool.get_id():
                 if tool.get_type() == "gripper" or tool.get_type() == "vacuum_pump":  # extra check for dynamixel tools
                     if not tool.is_connected():
-                        return self.create_response(400, "Correct ID but Dynamixel tool is not connected")
+                        response.status = 400
+                        response.message = "Correct ID but Dynamixel tool is not connected"
+                        return response
                 else:
                     self.ros_command_interface.ping_dxl_tool(0,
                                                              'No Dxl Tool')  # remove dxl tool if another kind of tool is selected
                 self.current_tool = tool
-                self.publish_current_tool_id(None)
-                return self.create_response(200, "New tool has been selected, id : " + str(new_tool_id))
+                self.publish_current_tool_id()
+                response.status = 200
+                response.message = "New tool has been selected, id : " + str(new_tool_id)
+                return response
 
         # no tool found in available tools
-        return self.create_response(400, "This ID does not match any available tool ID")
+        response.status = 400
+        response.message = "This ID does not match any available tool ID"
+        return response
 
     @staticmethod
     def create_action_result(status, message):

niryo_one_tools/niryo_one_tools/tool_ros_command_interface.py

@@ -26,6 +26,8 @@
 from niryo_one_msgs.srv import PushAirVacuumPump
 from niryo_one_msgs.srv import SetDigitalIO
 
+ROS_COMMUNICATION_PROBLEM = 0xA0
+
 
 class ToolRosCommandInterface:
 
@@ -53,36 +55,36 @@ def ping_dxl_tool(self, tool_id, tool_name):
                 tool_name = "No Dxl Tool"
             resp = self.service_ping_dxl_tool(tool_id, tool_name)
             return resp.state
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return ROS_COMMUNICATION_PROBLEM
 
     def open_gripper(self, gripper_id, open_position, open_speed, open_hold_torque):
         try:
             resp = self.service_open_gripper(gripper_id, open_position, open_speed, open_hold_torque)
             return resp.state
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return ROS_COMMUNICATION_PROBLEM
 
     def close_gripper(self, gripper_id, close_position, close_speed, close_hold_torque, close_max_torque):
         try:
             resp = self.service_close_gripper(gripper_id, close_position, close_speed, close_hold_torque,
                                               close_max_torque)
             return resp.state
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return ROS_COMMUNICATION_PROBLEM
 
     def pull_air_vacuum_pump(self, vp_id, vp_pull_air_position, vp_pull_air_hold_torque):
         try:
             resp = self.service_pull_air_vacuum_pump(vp_id, vp_pull_air_position, vp_pull_air_hold_torque)
             return resp.state
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return ROS_COMMUNICATION_PROBLEM
 
     def push_air_vacuum_pump(self, vp_id, vp_push_air_position):
         try:
             resp = self.service_push_air_vacuum_pump(vp_id, vp_push_air_position)
             return resp.state
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return ROS_COMMUNICATION_PROBLEM
 
     def digital_output_tool_setup(self, gpio_pin):
@@ -93,7 +95,7 @@ def digital_output_tool_setup(self, gpio_pin):
         try:
             resp = self.service_setup_digital_output_tool(gpio_pin, 0)  # set output
             return resp.status, resp.message
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return 400, "Digital IO panel service failed"
 
     def digital_output_tool_activate(self, gpio_pin, activate):
@@ -104,5 +106,5 @@ def digital_output_tool_activate(self, gpio_pin, activate):
         try:
             resp = self.service_activate_digital_output_tool(gpio_pin, activate)
             return resp.status, resp.message
-        except rclpy.ServiceException as e:
+        except Exception as e:
             return 400, "Digital IO panel service failed"