diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index 86f999fc5..a19a1158a 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -42,3 +42,6 @@ virtual_camera_plane_distance: 0.5 # The stitched image will be of size (, + row_padding) stitched_image_row_padding: 1182 + + # Change to True if missing gripper on arm + gripperless: False diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 815b330ad..242b174f7 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -248,6 +248,8 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.declare_parameter("spot_name", "") self.declare_parameter("mock_enable", False) + self.declare_parameter("gripperless", False) + # When we send very long trajectories to Spot, we create batches of # given size. If we do not batch a long trajectory, Spot will reject it. self.declare_parameter(self.TRAJECTORY_BATCH_SIZE_PARAM, 100) @@ -287,6 +289,8 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.graph_nav_seed_frame: str = self.get_parameter("graph_nav_seed_frame").value self.initialize_spot_cam: bool = self.get_parameter("initialize_spot_cam").value + self.gripperless: bool = self.get_parameter("gripperless").value + self._wait_for_goal: Optional[WaitForGoal] = None self.goal_handle: Optional[ServerGoalHandle] = None @@ -412,6 +416,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw port=self.port, logger=self.cam_logger, cert_resource_glob=self.certificate, + gripperless=self.gripperless, ) except SystemError: self.spot_cam_wrapper = None @@ -428,7 +433,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw has_arm = self.mock_has_arm if self.spot_wrapper is not None: has_arm = self.spot_wrapper.has_arm() - if has_arm: + if has_arm and not self.gripperless: all_cameras.append("hand") self.declare_parameter("cameras_used", all_cameras) self.cameras_used = self.get_parameter("cameras_used") @@ -564,22 +569,23 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw callback_group=self.group, ) - self.create_service( - Trigger, - "open_gripper", - lambda request, response: self.service_wrapper( - "open_gripper", self.handle_open_gripper, request, response - ), - callback_group=self.group, - ) - self.create_service( - Trigger, - "close_gripper", - lambda request, response: self.service_wrapper( - "close_gripper", self.handle_close_gripper, request, response - ), - callback_group=self.group, - ) + if not self.gripperless: + self.create_service( + Trigger, + "open_gripper", + lambda request, response: self.service_wrapper( + "open_gripper", self.handle_open_gripper, request, response + ), + callback_group=self.group, + ) + self.create_service( + Trigger, + "close_gripper", + lambda request, response: self.service_wrapper( + "close_gripper", self.handle_close_gripper, request, response + ), + callback_group=self.group, + ) self.create_service( SetBool, @@ -861,7 +867,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.handle_graph_nav_set_localization, callback_group=self.group, ) - if has_arm: + if has_arm and self.gripperless: self.create_service( GetGripperCameraParameters, "get_gripper_camera_parameters",