[SW-1142] Add cameras_used parameter (#423)

## Change Overview

Allows the user to specify the cameras they want to stream from in their config file. This can help alleviate some bandwidth problems from streaming from all cameras all the time. 

For example, to only stream from the frontleft, frontright, and hand cameras, you can add the following to the config yaml:
```
cameras_used: ["frontleft", "frontright", "hand"]
```

## Testing Done

- [x] when updating `cameras_used` in the config file, only those camera streams get published.
- [x] default launch with no specification of `cameras_used` still publishes all camera streams
- [x] stress testing with badly formed config yamls / no config file
    - [x] driver launches with all cameras if no config file is used
    - [x] if there are nonexistent cameras in the parameter `cameras_used` this logs a warning and defaults to all cameras used. This includes if the user specifies the hand camera when the robot doesn't have an arm
    - [x] image stitcher won't launch unless frontleft and frontright are also enabled (in addition to the launch argument `stitch_front_images` being set to true)
- [x] Test that things still work as expected on Spots without arms
- [x] update unit tests

README.md

@@ -91,7 +91,7 @@ See [`spot_examples`](spot_examples/) for some more complex examples of using th
 ## Images
 Perception data from Spot is handled through the `spot_image_publishers.launch.py` launchfile, which is launched by default from the driver. If you want to only view images from Spot, without bringing up any of the nodes to control the robot, you can also choose to run this launchfile independently.
 
-By default, the driver will publish RGB images as well as depth maps from the `frontleft`, `frontright`, `left`, `right`, and `back` cameras on Spot (plus `hand` if your Spot has an arm). If your Spot has greyscale cameras, you will need to set `rgb_cameras: False` in your configuration YAML file, or you will not recieve any image data. 
+By default, the driver will publish RGB images as well as depth maps from the `frontleft`, `frontright`, `left`, `right`, and `back` cameras on Spot (plus `hand` if your Spot has an arm). You can customize the cameras that are streamed from by adding the `cameras_used` parameter to your config yaml. (For example, to stream from only the front left and front right cameras, you can add `cameras_used: ["frontleft", "frontright"]`). Additionally, if your Spot has greyscale cameras, you will need to set `rgb_cameras: False` in your configuration YAML file, or you will not receive any image data.
 
 By default, the driver does not publish point clouds. To enable this, launch the driver with `publish_point_clouds:=True`.
 

spot_driver/config/spot_ros_example.yaml

@@ -27,6 +27,9 @@
     rgb_cameras: True  # Set to False if your robot has greyscale cameras -- otherwise you won't receive data.
     initialize_spot_cam: False # Set to True if you are connecting to a SpotCam payload module.
 
+    # You can uncomment and edit the list below if you only want to publish data from a certain set of cameras.
+    # cameras_used: ["frontleft", "frontright", "left", "right", "back", "hand"]
+
     # The following parameters are used in the image stitcher node and were determined through a lot of manual tuning.
     # They can be adjusted if the stitched image looks incorrect on your robot.
 

spot_driver/include/spot_driver/api/spot_image_sources.hpp

@@ -43,10 +43,10 @@ namespace spot_ros2 {
  * @param get_rgb_images Sets whether to request RGB images.
  * @param get_depth_images Sets whether to request depth images.
  * @param get_depth_registered_images Sets whether to request registered depth images.
- * @param has_hand_camera Sets whether to request images from the hand camera.
+ * @param spot_cameras_used Set of SpotCameras to stream from.
  * @return A set of ImageSources which represents all requested image and camera types.
  */
 [[nodiscard]] std::set<ImageSource> createImageSources(const bool get_rgb_images, const bool get_depth_images,
                                                        const bool get_depth_registered_images,
-                                                       const bool has_hand_camera);
+                                                       const std::set<SpotCamera>& spot_cameras_used);
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -3,7 +3,11 @@
 #pragma once
 
 #include <optional>
+#include <set>
 #include <string>
+#include <tl_expected/expected.hpp>
+
+#include <spot_driver/types.hpp>
 
 namespace spot_ros2 {
 /**
@@ -37,6 +41,8 @@ class ParameterInterfaceBase {
   virtual bool getPublishDepthRegisteredImages() const = 0;
   virtual std::string getPreferredOdomFrame() const = 0;
   virtual std::string getSpotName() const = 0;
+  virtual std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(bool has_arm) const = 0;
+  virtual tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(bool has_arm) const = 0;
 
  protected:
   // These are the definitions of the default values for optional parameters.
@@ -51,5 +57,7 @@ class ParameterInterfaceBase {
   static constexpr bool kDefaultPublishDepthImages{true};
   static constexpr bool kDefaultPublishDepthRegisteredImages{true};
   static constexpr auto kDefaultPreferredOdomFrame = "odom";
+  static constexpr auto kDefaultCamerasUsedWithoutArm = {"frontleft", "frontright", "left", "right", "back"};
+  static constexpr auto kDefaultCamerasUsedWithArm = {"frontleft", "frontright", "left", "right", "back", "hand"};
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -7,7 +7,9 @@
 
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
+#include <vector>
 
 namespace spot_ros2 {
 /**
@@ -34,6 +36,9 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
   [[nodiscard]] bool getPublishDepthRegisteredImages() const override;
   [[nodiscard]] std::string getPreferredOdomFrame() const override;
   [[nodiscard]] std::string getSpotName() const override;
+  [[nodiscard]] std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm) const override;
+  [[nodiscard]] tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(
+      const bool has_arm) const override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/include/spot_driver/types.hpp

@@ -21,6 +21,8 @@
 
 #include <functional>
 #include <optional>
+#include <string>
+#include <unordered_map>
 
 namespace spot_ros2 {
 /** @brief Represents the six different cameras on Spot. */
@@ -33,6 +35,18 @@ enum class SpotCamera {
   HAND,
 };
 
+/**
+ * @brief Map from each ROS camera topic name to SpotCamera value.
+ */
+static const std::unordered_map<std::string, spot_ros2::SpotCamera> kRosStringToSpotCamera{
+    {"back", spot_ros2::SpotCamera::BACK},
+    {"frontleft", spot_ros2::SpotCamera::FRONTLEFT},
+    {"frontright", spot_ros2::SpotCamera::FRONTRIGHT},
+    {"hand", spot_ros2::SpotCamera::HAND},
+    {"left", spot_ros2::SpotCamera::LEFT},
+    {"right", spot_ros2::SpotCamera::RIGHT},
+};
+
 /** @brief Represents the three types of images Spot can capture. */
 enum class SpotImageType {
   RGB,

spot_driver/launch/spot_image_publishers.launch.py

@@ -11,7 +11,7 @@
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
-from spot_driver.launch.spot_launch_helpers import spot_has_arm
+from spot_driver.launch.spot_launch_helpers import get_camera_sources, spot_has_arm
 
 
 class DepthRegisteredMode(Enum):
@@ -23,24 +23,17 @@ def __repr__(self) -> str:
         return self.value
 
 
-def get_camera_sources(has_arm: bool) -> List[str]:
-    camera_sources = ["frontleft", "frontright", "left", "right", "back"]
-    if has_arm:
-        camera_sources.append("hand")
-    return camera_sources
-
-
 def create_depth_registration_nodelets(
     context: launch.LaunchContext,
     spot_name: LaunchConfiguration,
-    has_arm: bool,
+    camera_sources: List[str],
 ) -> List[launch_ros.descriptions.ComposableNode]:
     """Create the list of depth_image_proc::RegisterNode composable nodes required to generate registered depth images
     for Spot's cameras."""
 
     composable_node_descriptions = []
 
-    for camera in get_camera_sources(has_arm):
+    for camera in camera_sources:
         composable_node_descriptions.append(
             launch_ros.descriptions.ComposableNode(
                 package="depth_image_proc",
@@ -71,14 +64,14 @@ def create_depth_registration_nodelets(
 def create_point_cloud_nodelets(
     context: launch.LaunchContext,
     spot_name: LaunchConfiguration,
-    has_arm: bool,
+    camera_sources: List[str],
 ) -> List[launch_ros.descriptions.ComposableNode]:
     """Create the list of depth_image_proc::PointCloudXyzrgbNode composable nodes required to generate point clouds for
     each pair of RGB and registered depth cameras."""
 
     composable_node_descriptions = []
 
-    for camera in get_camera_sources(has_arm):
+    for camera in camera_sources:
         composable_node_descriptions.append(
             launch_ros.descriptions.ComposableNode(
                 package="depth_image_proc",
@@ -120,6 +113,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     else:
         has_arm = spot_has_arm(config_file_path=config_file.perform(context), spot_name=spot_name)
 
+    camera_sources = get_camera_sources(config_file_path, has_arm)
+
     depth_registered_mode_string = depth_registered_mode_config.perform(context).lower()
     depth_registered_mode = DepthRegisteredMode(depth_registered_mode_string)
 
@@ -152,10 +147,10 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     # Parse config options to create a list of composable node descriptions for the nodelets we want to run within the
     # composable node container.
     composable_node_descriptions = (
-        create_depth_registration_nodelets(context, spot_name, has_arm)
+        create_depth_registration_nodelets(context, spot_name, camera_sources)
         if depth_registered_mode is DepthRegisteredMode.FROM_NODELETS
         else []
-    ) + (create_point_cloud_nodelets(context, spot_name, has_arm) if publish_point_clouds else [])
+    ) + (create_point_cloud_nodelets(context, spot_name, camera_sources) if publish_point_clouds else [])
     container = launch_ros.actions.ComposableNodeContainer(
         name="container",
         namespace=spot_name,
@@ -166,28 +161,29 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     )
     ld.add_action(container)
 
-    # add the image stitcher node
-    stitcher_params = {
-        "body_frame": f"{spot_name}/body" if spot_name else "body",
-        "virtual_camera_frame": f"{spot_name}/virtual_camera" if spot_name else "virtual_camera",
-    }
-    stitcher_prefix = f"/{spot_name}" if spot_name else ""
-    image_stitcher_node = launch_ros.actions.Node(
-        package="spot_driver",
-        executable="image_stitcher_node",
-        namespace=spot_name,
-        output="screen",
-        remappings=[
-            (f"{stitcher_prefix}/left/image", f"{stitcher_prefix}/camera/frontleft/image"),
-            (f"{stitcher_prefix}/left/camera_info", f"{stitcher_prefix}/camera/frontleft/camera_info"),
-            (f"{stitcher_prefix}/right/image", f"{stitcher_prefix}/camera/frontright/image"),
-            (f"{stitcher_prefix}/right/camera_info", f"{stitcher_prefix}/camera/frontright/camera_info"),
-            (f"{stitcher_prefix}/virtual_camera/image", f"{stitcher_prefix}/camera/frontmiddle_virtual/image"),
-        ],
-        parameters=[config_file, stitcher_params],
-        condition=IfCondition(LaunchConfiguration("stitch_front_images")),
-    )
-    ld.add_action(image_stitcher_node)
+    # add the image stitcher node, but only if frontleft and frontright cameras are enabled.
+    if "frontleft" in camera_sources and "frontright" in camera_sources:
+        stitcher_params = {
+            "body_frame": f"{spot_name}/body" if spot_name else "body",
+            "virtual_camera_frame": f"{spot_name}/virtual_camera" if spot_name else "virtual_camera",
+        }
+        stitcher_prefix = f"/{spot_name}" if spot_name else ""
+        image_stitcher_node = launch_ros.actions.Node(
+            package="spot_driver",
+            executable="image_stitcher_node",
+            namespace=spot_name,
+            output="screen",
+            remappings=[
+                (f"{stitcher_prefix}/left/image", f"{stitcher_prefix}/camera/frontleft/image"),
+                (f"{stitcher_prefix}/left/camera_info", f"{stitcher_prefix}/camera/frontleft/camera_info"),
+                (f"{stitcher_prefix}/right/image", f"{stitcher_prefix}/camera/frontright/image"),
+                (f"{stitcher_prefix}/right/camera_info", f"{stitcher_prefix}/camera/frontright/camera_info"),
+                (f"{stitcher_prefix}/virtual_camera/image", f"{stitcher_prefix}/camera/frontmiddle_virtual/image"),
+            ],
+            parameters=[config_file, stitcher_params],
+            condition=IfCondition(LaunchConfiguration("stitch_front_images")),
+        )
+        ld.add_action(image_stitcher_node)
 
 
 def generate_launch_description() -> launch.LaunchDescription:

spot_driver/spot_driver/launch/spot_launch_helpers.py

@@ -2,12 +2,47 @@
 
 import logging
 import os
-from typing import Optional, Tuple
+from typing import Any, Dict, List, Optional, Tuple
 
 import yaml
 
 from spot_wrapper.wrapper import SpotWrapper
 
+COLOR_END = "\33[0m"
+COLOR_YELLOW = "\33[33m"
+
+
+def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]:
+    """Get a dictionary of parameter_name: parameter_value from a ROS config yaml file.
+
+    Args:
+        config_file_path (str): Path to the config yaml.
+
+    Raises:
+        YamlError: If the yaml can't be parsed
+        ValueError: If the yaml file doesn't follow ROS conventions
+
+    Returns:
+        dict[str, Any]: dictionary of parameter_name: parameter_value.
+        If there is no config file, returns the empty dictionary.
+    """
+    # an empty config file path is a valid way to start the driver, so here we just return the empty dictionary.
+    if not config_file_path:
+        return {}
+    with open(config_file_path, "r") as config_yaml:
+        try:
+            config_dict = yaml.safe_load(config_yaml)
+            if ("/**" in config_dict) and ("ros__parameters" in config_dict["/**"]):
+                ros_params = config_dict["/**"]["ros__parameters"]
+                return ros_params
+            else:
+                raise ValueError(
+                    "Your yaml file does not follow ROS conventions! Make sure it starts with '/**' and"
+                    " 'ros__parameters'."
+                )
+        except yaml.YAMLError as exc:
+            raise yaml.YAMLError(f"Config file {config_file_path} couldn't be parsed: failed with '{exc}'")
+
 
 def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str]]:
     """Obtain the username, password, hostname, port, and certificate of Spot from the environment variables or,
@@ -30,35 +65,76 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional
     port = int(portnum) if portnum else None
     certificate = os.getenv("SPOT_CERTIFICATE")
 
-    # parse the yaml to determine if login information is set there
-    if os.path.isfile(config_file_path):
-        with open(config_file_path, "r") as config_yaml:
-            try:
-                config_dict = yaml.safe_load(config_yaml)
-                if ("/**" in config_dict) and ("ros__parameters" in config_dict["/**"]):
-                    ros_params = config_dict["/**"]["ros__parameters"]
-                    # only set username/password/hostname if they were not already set as environment variables.
-                    if (not username) and ("username" in ros_params):
-                        username = ros_params["username"]
-                    if (not password) and ("password" in ros_params):
-                        password = ros_params["password"]
-                    if (not hostname) and ("hostname" in ros_params):
-                        hostname = ros_params["hostname"]
-                    if (not port) and ("port" in ros_params):
-                        port = ros_params["port"]
-                    if (not certificate) and ("certificate" in ros_params):
-                        certificate = ros_params["certificate"]
-            except yaml.YAMLError as exc:
-                print("Parsing config_file yaml failed with: {}".format(exc))
+    ros_params = get_ros_param_dict(config_file_path)
+    # only set username/password/hostname if they were not already set as environment variables.
+    if not username and "username" in ros_params:
+        username = ros_params["username"]
+    if not password and "password" in ros_params:
+        password = ros_params["password"]
+    if not hostname and "hostname" in ros_params:
+        hostname = ros_params["hostname"]
+    if not port and "port" in ros_params:
+        port = ros_params["port"]
+    if not certificate and "certificate" in ros_params:
+        certificate = ros_params["certificate"]
     if (not username) or (not password) or (not hostname):
         raise ValueError(
             "One or more of your login credentials has not been specified! Got invalid values of "
-            "[Username: '{}' Password: '{}' Hostname: '{}']. Ensure that your environment variables are set or "
-            "update your config_file yaml.".format(username, password, hostname)
+            f"[Username: '{username}' Password: '{password}' Hostname: '{hostname}']. Ensure that your environment "
+            "variables are set or update your config_file yaml."
         )
     return username, password, hostname, port, certificate
 
 
+def default_camera_sources(has_arm: bool) -> List[str]:
+    camera_sources = ["frontleft", "frontright", "left", "right", "back"]
+    if has_arm:
+        camera_sources.append("hand")
+    return camera_sources
+
+
+def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool) -> List[str]:
+    """Get the list of cameras to stream from. This will be taken from the parameters in the config yaml if it exists
+    and contains valid cameras. If this list contains invalid cameras, fall back to the default of all cameras.
+
+    Args:
+        ros_params (str): Dictionary of ros parameters from the config file.
+        has_arm (bool): Whether or not your Spot has an arm.
+
+    Raises:
+        ValueError: If the parameter cameras_used is not formattted as a list.
+
+    Returns:
+        List[str]: List of cameras the driver will stream from.
+    """
+    default_sources = default_camera_sources(has_arm)
+    if "cameras_used" in ros_params:
+        camera_sources = ros_params["cameras_used"]
+        if isinstance(camera_sources, List):
+            # check if the user inputted any camera that's not in the default sources.
+            invalid_cameras = [cam for cam in camera_sources if cam not in default_sources]
+            if invalid_cameras:
+                print(
+                    f"{COLOR_YELLOW}WARNING: Your camera sources {camera_sources} contain invalid cameras. Make sure"
+                    f" that the values are a subset of the default {default_sources}. Falling back to these default"
+                    f" sources instead.{COLOR_END}"
+                )
+                return default_sources
+            return camera_sources
+        else:
+            raise ValueError(f"Your camera sources {camera_sources} are not formatted correctly as a list!")
+    else:
+        return default_sources
+
+
+def get_camera_sources(config_file_path: str, has_arm: bool) -> List[str]:
+    """Wrapper around get_camera_sources_from_ros_params that grabs the ros parameters from the config file."""
+    camera_sources = get_camera_sources_from_ros_params(get_ros_param_dict(config_file_path), has_arm)
+    if len(camera_sources) == 0:
+        print(f"{COLOR_YELLOW}WARNING: No camera sources are selected. Was this intended?{COLOR_END}")
+    return camera_sources
+
+
 def spot_has_arm(config_file_path: str, spot_name: str) -> bool:
     """Check if Spot has an arm querying the robot through SpotWrapper