From e51722d0bb6139fa6b03001fd45a2ceac8e21f8a Mon Sep 17 00:00:00 2001 From: Gustavo Nunes Goretkin <124000349+ggoretkin-bdai@users.noreply.github.com> Date: Fri, 8 Dec 2023 13:43:36 -0500 Subject: [PATCH] [N/A] Add a mock launch argument (#199) This PR uses node / launch arguments to specify whether the driver should operate in "mock" mode, instead of using the sentinel name `Mock_spot`. With an explicit parameter, we can have multiple mocks, with different names, and also affect whether there is an arm. For example, with this modification you can do ``` cd spot_driver colcon build --symlink # (`symlink` [does not apply to launch files](https://github.com/colcon/colcon-core/issues/407)) source install/setup.bash ros2 launch spot_driver spot_driver.launch.py spot_name:=AnyNameHere mock_enable:=True mock_has_arm:=True ``` and then, for example (`spot_utilities`, specifically in `bdai` repo) ``` # source so that `spot_driver` can be found cd spot_utilities # /workspaces/bdai/ws/src/spot_utilities colcon build source install/setup.bash ros2 run spot_utilities spot_teleop.py AnyNameHere ``` --- spot_driver/launch/spot_driver.launch.py | 32 +++++++++++------ spot_driver/spot_driver/spot_ros2.py | 45 ++++++++++++++++++------ spot_driver/test/test_spot_driver.py | 10 ++++-- 3 files changed, 64 insertions(+), 23 deletions(-) diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index f040195fd..3c01f20be 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -114,15 +114,21 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: tf_prefix = LaunchConfiguration("tf_prefix").perform(context) depth_registered_mode_config = LaunchConfiguration("depth_registered_mode") publish_point_clouds_config = LaunchConfiguration("publish_point_clouds") - - # Get parameters from Spot. - - username = os.getenv("BOSDYN_CLIENT_USERNAME", "username") - password = os.getenv("BOSDYN_CLIENT_PASSWORD", "password") - hostname = os.getenv("SPOT_IP", "hostname") - - spot_wrapper = SpotWrapper(username, password, hostname, spot_name, logger) - has_arm = spot_wrapper.has_arm() + mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context) + + if not mock_enable: + # Get parameters from Spot. + # TODO this deviates from the `get_from_env_and_fall_back_to_param` logic in `spot_ros2.py`, + # which would pull in values in `config_file` + username = os.getenv("BOSDYN_CLIENT_USERNAME", "username") + password = os.getenv("BOSDYN_CLIENT_PASSWORD", "password") + hostname = os.getenv("SPOT_IP", "hostname") + + spot_wrapper = SpotWrapper(username, password, hostname, spot_name, logger) + has_arm = spot_wrapper.has_arm() + else: + mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) + has_arm = mock_has_arm pkg_share = FindPackageShare("spot_description").find("spot_description") @@ -154,11 +160,17 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: # in spot_driver. spot_driver_params = { "spot_name": spot_name, + "mock_enable": mock_enable, "publish_depth_registered": False, "publish_depth": False, "publish_rgb": False, } + if mock_enable: + mock_spot_driver_params = {"mock_has_arm": mock_has_arm} + # Merge the two dicts + spot_driver_params = {**spot_driver_params, **mock_spot_driver_params} + spot_driver_node = launch_ros.actions.Node( package="spot_driver", executable="spot_ros2", @@ -196,7 +208,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: PathJoinSubstitution([pkg_share, "urdf", "spot.urdf.xacro"]), " ", "arm:=", - TextSubstitution(text=str(spot_wrapper.has_arm()).lower()), + TextSubstitution(text=str(has_arm).lower()), " ", "tf_prefix:=", tf_prefix, diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 001a684a8..f6a0f2344 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -139,7 +139,6 @@ from spot_wrapper.wrapper import SpotWrapper MAX_DURATION = 1e6 -MOCK_HOSTNAME = "Mock_spot" COLOR_END = "\33[0m" COLOR_GREEN = "\33[32m" COLOR_YELLOW = "\33[33m" @@ -194,6 +193,14 @@ class SpotImageType(str, Enum): RegDepth = "depth_registered" +def set_node_parameter_from_parameter_list( + node: Node, parameter_list: Optional[typing.List[Parameter]], parameter_name: str +) -> None: + """Set parameters when the node starts not from a launch file.""" + if parameter_list is not None: + node.set_parameters([parameter for parameter in parameter_list if parameter.name == parameter_name]) + + class SpotROS(Node): """Parent class for using the wrapper. Defines all callbacks and keeps the wrapper alive""" @@ -253,6 +260,12 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.declare_parameter("initialize_spot_cam", False) self.declare_parameter("spot_name", "") + self.declare_parameter("mock_enable", False) + + # If `mock_enable:=True`, then there are additional parameters. We must set this one separately. + set_node_parameter_from_parameter_list(self, parameter_list, "mock_enable") + if self.get_parameter("mock_enable").value: + self.declare_parameter("mock_has_arm", rclpy.Parameter.Type.BOOL) # used for setting when not using launch file if parameter_list is not None: @@ -294,6 +307,10 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.name: Optional[str] = self.get_parameter("spot_name").value if not self.name: self.name = None + self.mock: bool = self.get_parameter("mock_enable").value + self.mock_has_arm: Optional[bool] = None + if self.mock: + self.mock_has_arm = self.get_parameter("mock_has_arm").value self.motion_deadzone: Parameter = self.get_parameter("deadzone") self.estop_timeout: Parameter = self.get_parameter("estop_timeout") @@ -363,10 +380,11 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw name_str = "" if self.name is not None: name_str = " for " + self.name - self.get_logger().info("Starting ROS driver for Spot" + name_str) + mocking_designator = " (mocked)" if self.mock else "" + self.get_logger().info("Starting ROS driver for Spot" + name_str + mocking_designator) # testing with Robot - if self.name == MOCK_HOSTNAME: + if self.mock: self.spot_wrapper: Optional[SpotWrapper] = None self.cam_wrapper: Optional[SpotCamWrapper] = None else: @@ -397,8 +415,16 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw except SystemError: self.spot_cam_wrapper = None + if self.frame_prefix != self.spot_wrapper.frame_prefix: + error_msg = ( + f"ERROR: disagreement between `self.frame_prefix` ({self.frame_prefix}) and" + f" `self.spot_wrapper.frame_prefix` ({self.spot_wrapper.frame_prefix})" + ) + self.get_logger().error(error_msg) + raise ValueError(error_msg) + all_cameras = ["frontleft", "frontright", "left", "right", "back"] - has_arm = False + has_arm = self.mock_has_arm if self.spot_wrapper is not None: has_arm = self.spot_wrapper.has_arm() if has_arm: @@ -2365,13 +2391,10 @@ def populate_camera_static_transforms(self, image_data: image_pb2.Image) -> None # We exclude the odometry frames from static transforms since they are not static. We can ignore the body # frame because it is a child of odom or vision depending on the preferred_odom_frame, and will be published # by the non-static transform publishing that is done by the state callback - frame_prefix = MOCK_HOSTNAME + "/" - if self.spot_wrapper is not None: - frame_prefix = self.spot_wrapper.frame_prefix excluded_frames = [ self.tf_name_vision_odom.value, self.tf_name_kinematic_odom.value, - frame_prefix + "body", + self.frame_prefix + "body", ] excluded_frames = [f[f.rfind("/") + 1 :] for f in excluded_frames] @@ -2405,8 +2428,8 @@ def populate_camera_static_transforms(self, image_data: image_pb2.Image) -> None (transform.header.frame_id, transform.child_frame_id) for transform in self.camera_static_transforms ] if ( - frame_prefix + parent_frame, - frame_prefix + frame_name, + self.frame_prefix + parent_frame, + self.frame_prefix + frame_name, ) in existing_transforms: # We already extracted this transform continue @@ -2421,7 +2444,7 @@ def populate_camera_static_transforms(self, image_data: image_pb2.Image) -> None parent_frame, frame_name, transform.parent_tform_child, - frame_prefix, + self.frame_prefix, ) self.camera_static_transforms.append(static_tf) self.camera_static_transform_broadcaster.sendTransform(self.camera_static_transforms) diff --git a/spot_driver/test/test_spot_driver.py b/spot_driver/test/test_spot_driver.py index 8922274c0..3ccecc4e7 100644 --- a/spot_driver/test/test_spot_driver.py +++ b/spot_driver/test/test_spot_driver.py @@ -18,8 +18,14 @@ def setUp(self) -> None: self.fixture = contextlib.ExitStack() self.ros = self.fixture.enter_context(ros_scope.top(namespace="fixture")) # create and run spot ros2 servers - mock_param = rclpy.parameter.Parameter("spot_name", rclpy.Parameter.Type.STRING, "Mock_spot") - self.spot_ros2 = self.ros.load(spot_driver.spot_ros2.SpotROS, parameter_list=[mock_param]) + self.spot_ros2 = self.ros.load( + spot_driver.spot_ros2.SpotROS, + parameter_list=[ + rclpy.parameter.Parameter("spot_name", value="Mock_spot"), + rclpy.parameter.Parameter("mock_enable", value=True), + rclpy.parameter.Parameter("mock_has_arm", value=False), + ], + ) # clients self.claim_client: rclpy.node.Client = self.ros.node.create_client(Trigger, "claim")