diff --git a/niryo_one_tools/niryo_one_tools/tool_controller.py b/niryo_one_tools/niryo_one_tools/tool_controller.py index 676e3bb..9211aa3 100644 --- a/niryo_one_tools/niryo_one_tools/tool_controller.py +++ b/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): diff --git a/niryo_one_tools/niryo_one_tools/tool_ros_command_interface.py b/niryo_one_tools/niryo_one_tools/tool_ros_command_interface.py index c3af8d8..b440c1a 100644 --- a/niryo_one_tools/niryo_one_tools/tool_ros_command_interface.py +++ b/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,14 +55,14 @@ 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): @@ -68,21 +70,21 @@ def close_gripper(self, gripper_id, close_position, close_speed, close_hold_torq 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"