[SW-1539] don't publish images from hand camera if gripperless param set

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -41,8 +41,10 @@ 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;
+  virtual bool getGripperless() const = 0;
+  virtual std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0;
+  virtual tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(bool has_arm,
+                                                                                    bool gripperless) const = 0;
 
  protected:
   // These are the definitions of the default values for optional parameters.
@@ -57,7 +59,8 @@ 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"};
+  static constexpr bool kDefaultGripperless{false};
+  static constexpr auto kCamerasWithoutHand = {"frontleft", "frontright", "left", "right", "back"};
+  static constexpr auto kCamerasWithHand = {"frontleft", "frontright", "left", "right", "back", "hand"};
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -36,9 +36,11 @@ 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]] bool getGripperless() const override;
+  [[nodiscard]] std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm,
+                                                                      const bool gripperless) const override;
   [[nodiscard]] tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(
-      const bool has_arm) const override;
+      const bool has_arm, const bool gripperless) const override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

spot_driver/spot_driver/launch/spot_launch_helpers.py

@@ -170,9 +170,9 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional
     return username, password, hostname, port, certificate
 
 
-def default_camera_sources(has_arm: bool) -> List[str]:
+def default_camera_sources(has_arm: bool, gripperless: bool) -> List[str]:
     camera_sources = ["frontleft", "frontright", "left", "right", "back"]
-    if has_arm:
+    if has_arm and not gripperless:
         camera_sources.append("hand")
     return camera_sources
 
@@ -191,7 +191,11 @@ def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool
     Returns:
         List[str]: List of cameras the driver will stream from.
     """
-    default_sources = default_camera_sources(has_arm)
+    gripperless = False
+    if "gripperless" in ros_params:
+        if isinstance(ros_params["gripperless"], bool):
+            gripperless = ros_params["gripperless"]
+    default_sources = default_camera_sources(has_arm, gripperless)
     if "cameras_used" in ros_params:
         camera_sources = ros_params["cameras_used"]
         if isinstance(camera_sources, List):

spot_driver/src/images/spot_image_publisher.cpp

@@ -89,15 +89,16 @@ bool SpotImagePublisher::initialize() {
   const auto publish_raw_rgb_cameras = false;
   const auto uncompress_images = parameters_->getUncompressImages();
   const auto publish_compressed_images = parameters_->getPublishCompressedImages();
+  const auto gripperless = parameters_->getGripperless();
 
   std::set<spot_ros2::SpotCamera> cameras_used;
-  const auto cameras_used_parameter = parameters_->getCamerasUsed(has_arm_);
+  const auto cameras_used_parameter = parameters_->getCamerasUsed(has_arm_, gripperless);
   if (cameras_used_parameter.has_value()) {
     cameras_used = cameras_used_parameter.value();
   } else {
     logger_->logWarn("Invalid cameras_used parameter! Got error: " + cameras_used_parameter.error() +
                      " Defaulting to publishing from all cameras.");
-    cameras_used = parameters_->getDefaultCamerasUsed(has_arm_);
+    cameras_used = parameters_->getDefaultCamerasUsed(has_arm_, gripperless);
   }
 
   // Generate the set of image sources based on which cameras the user has requested that we publish

spot_driver/src/interfaces/rclcpp_parameter_interface.cpp

@@ -26,6 +26,7 @@ constexpr auto kParameterNamePublishCompressedImages = "publish_compressed_image
 constexpr auto kParameterNamePublishDepthImages = "publish_depth";
 constexpr auto kParameterNamePublishDepthRegisteredImages = "publish_depth_registered";
 constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame";
+constexpr auto kParameterNameGripperless = "gripperless";
 
 /**
  * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and
@@ -186,8 +187,14 @@ std::string RclcppParameterInterface::getPreferredOdomFrame() const {
   return declareAndGetParameter<std::string>(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame);
 }
 
-std::set<spot_ros2::SpotCamera> RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm) const {
-  const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm;
+bool RclcppParameterInterface::getGripperless() const {
+  return declareAndGetParameter<bool>(node_, kParameterNameGripperless, kDefaultGripperless);
+}
+
+std::set<spot_ros2::SpotCamera> RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm,
+                                                                                const bool gripperless) const {
+  const bool has_hand_camera = has_arm && (!gripperless);
+  const auto kDefaultCamerasUsed = (has_hand_camera) ? kCamerasWithHand : kCamerasWithoutHand;
   std::set<spot_ros2::SpotCamera> spot_cameras_used;
   for (const auto& camera : kDefaultCamerasUsed) {
     spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera)));
@@ -196,8 +203,9 @@ std::set<spot_ros2::SpotCamera> RclcppParameterInterface::getDefaultCamerasUsed(
 }
 
 tl::expected<std::set<spot_ros2::SpotCamera>, std::string> RclcppParameterInterface::getCamerasUsed(
-    const bool has_arm) const {
-  const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm;
+    const bool has_arm, const bool gripperless) const {
+  const bool has_hand_camera = has_arm && (!gripperless);
+  const auto kDefaultCamerasUsed = (has_hand_camera) ? kCamerasWithHand : kCamerasWithoutHand;
   const std::vector<std::string> kDefaultCamerasUsedVector(std::begin(kDefaultCamerasUsed),
                                                            std::end(kDefaultCamerasUsed));
   const auto cameras_used_param =
@@ -208,6 +216,8 @@ tl::expected<std::set<spot_ros2::SpotCamera>, std::string> RclcppParameterInterf
       const auto spot_camera = kRosStringToSpotCamera.at(camera);
       if ((spot_camera == SpotCamera::HAND) && (!has_arm)) {
         return tl::make_unexpected("Cannot add SpotCamera 'hand', the robot does not have an arm!");
+      } else if ((spot_camera == SpotCamera::HAND) && gripperless) {
+        return tl::make_unexpected("Cannot add SpotCamera 'hand', the robot is gripperless!");
       }
       spot_cameras_used.insert(spot_camera);
     } catch (const std::out_of_range& e) {

spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp

@@ -40,17 +40,20 @@ class FakeParameterInterface : public ParameterInterfaceBase {
 
   std::string getSpotName() const override { return spot_name; }
 
-  std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm) const override {
-    const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm;
+  bool getGripperless() const override { return gripperless; }
+
+  std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm, const bool gripperless) const override {
+    const auto kDefaultCamerasUsed = (has_arm && !gripperless) ? kCamerasWithHand : kCamerasWithoutHand;
     std::set<spot_ros2::SpotCamera> spot_cameras_used;
     for (const auto& camera : kDefaultCamerasUsed) {
       spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera)));
     }
     return spot_cameras_used;
   }
 
-  tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(const bool has_arm) const override {
-    return getDefaultCamerasUsed(has_arm);
+  tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(const bool has_arm,
+                                                                            const bool gripperless) const override {
+    return getDefaultCamerasUsed(has_arm, gripperless);
   }
 
   static constexpr auto kExampleHostname{"192.168.0.10"};
@@ -64,6 +67,7 @@ class FakeParameterInterface : public ParameterInterfaceBase {
   bool publish_rgb_images = ParameterInterfaceBase::kDefaultPublishRGBImages;
   bool publish_depth_images = ParameterInterfaceBase::kDefaultPublishDepthImages;
   bool publish_depth_registered_images = ParameterInterfaceBase::kDefaultPublishDepthRegisteredImages;
+  bool gripperless = ParameterInterfaceBase::kDefaultGripperless;
   std::string spot_name;
 };
 }  // namespace spot_ros2::test

spot_driver/test/src/test_parameter_interface.cpp

@@ -279,9 +279,13 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) {
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
 
+  // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware
+  bool arm = true;
+  bool gripperless = false;
+
   // WHEN we call the functions to get the config values from the parameter interface
   // THEN we get the default of all available cameras.
-  const auto cameras_used_arm = parameter_interface.getCamerasUsed(true);
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_arm.has_value(), IsTrue());
   EXPECT_THAT(cameras_used_arm.value(),
               UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, SpotCamera::LEFT, SpotCamera::RIGHT,
@@ -296,9 +300,13 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithoutArm) {
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
 
+  // GIVEN we are operating on a robot without an arm, and without custom gripperless firmware
+  bool arm = false;
+  bool gripperless = false;
+
   // WHEN we call the functions to get the config values from the parameter interface
   // THEN we get the default of all available cameras.
-  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false);
+  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue());
   EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT,
                                                                 SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK));
@@ -312,13 +320,22 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedSubset) {
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
 
+  // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware
+  bool arm = true;
+  bool gripperless = false;
+
   // WHEN we call the functions to get the config values from the parameter interface
-  // THEN the returned values match the values we used when declaring the parameters, regardless of if there is an arm
-  const auto cameras_used_arm = parameter_interface.getCamerasUsed(true);
+  // THEN the returned values match the values we used when declaring the parameters
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_arm.has_value(), IsTrue());
   EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT));
 
-  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false);
+  // GIVEN we are operating on a robot without an arm
+  arm = false;
+
+  // WHEN we call the functions to get the config values from the parameter interface
+  // THEN the returned values match the values we used when declaring the parameters
+  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue());
   EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT));
 }
@@ -331,16 +348,21 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedSubsetWithHand) {
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
 
+  // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware
+  bool arm = true;
+  bool gripperless = false;
+
   // WHEN we call the functions to get the config values from the parameter interface if the robot has an arm
   // THEN the returned values match the values we used when declaring the parameters
-  const auto cameras_used_arm = parameter_interface.getCamerasUsed(true);
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_arm.has_value(), IsTrue());
   EXPECT_THAT(cameras_used_arm.value(),
               UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, SpotCamera::HAND));
 
   // WHEN we call the functions to get the config values from the parameter interface if the robot does not have an arm
   // THEN this is an invalid choice of parameters.
-  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false);
+  arm = false;
+  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_no_arm.has_value(), IsFalse());
   EXPECT_THAT(cameras_used_no_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot does not have an arm!"));
 }
@@ -353,13 +375,62 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedWithInvalidCamera) {
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
 
+  // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware
+  bool arm = true;
+  bool gripperless = false;
+
   // WHEN we call the functions to get the config values from the parameter interface
   // THEN the result is invalid for robots with and without arms, as the camera "not_a_camera" does not exist on Spot.
-  const auto cameras_used_arm = parameter_interface.getCamerasUsed(true);
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_arm.has_value(), IsFalse());
   EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot convert camera 'not_a_camera' to a SpotCamera."));
-  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false);
+  arm = false;
+  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless);
   EXPECT_THAT(cameras_used_no_arm.has_value(), IsFalse());
   EXPECT_THAT(cameras_used_no_arm.error(), StrEq("Cannot convert camera 'not_a_camera' to a SpotCamera."));
 }
+
+TEST_F(RclcppParameterInterfaceEnvVarTest, GetDefaultCamerasUsedGripperless) {
+  // GIVEN we create a RclcppParameterInterface using the node
+  RclcppParameterInterface parameter_interface{node_};
+
+  // GIVEN we are operating on a robot with an arm, and WITH custom gripperless firmware
+  bool arm = true;
+  bool gripperless = true;
+
+  // WHEN we call the functions to get the config values from the parameter interface
+  // THEN we get the default of all available cameras, excluding the hand!
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
+  EXPECT_THAT(cameras_used_arm.has_value(), IsTrue());
+  EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT,
+                                                             SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK));
+
+  // WHEN gripperless is set to true on a robot without an arm
+  // THEN we still get the default of all available cameras, excluding the hand
+  arm = false;
+  const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless);
+  EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue());
+  EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT,
+                                                                SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK));
+}
+
+TEST_F(RclcppParameterInterfaceEnvVarTest, GetSelectedCamerasUsedGripperless) {
+  // GIVEN we set cameras used to a subset of the available cameras including the hand camera
+  const std::vector<std::string> cameras_used_parameter = {"frontleft", "frontright", "hand"};
+  node_->declare_parameter("cameras_used", cameras_used_parameter);
+
+  // GIVEN we create a RclcppParameterInterface using the node
+  RclcppParameterInterface parameter_interface{node_};
+
+  // GIVEN we are operating on a robot with an arm, and WITH custom gripperless firmware
+  bool arm = true;
+  bool gripperless = true;
+
+  // WHEN we call the functions to get the config values from the parameter interface
+  // THEN this is an invalid choice of parameters, as the hand camera is not available on gripperless robots.
+  const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless);
+  EXPECT_THAT(cameras_used_arm.has_value(), IsFalse());
+  EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot is gripperless!"));
+}
+
 }  // namespace spot_ros2::test