[N/A] Add a mock launch argument (bdaiinstitute#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](colcon/colcon-core#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

@@ -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,

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)

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")