From feea7b67a9e9890e8772b165dfbbbc30ecb8ebb2 Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Fri, 1 Nov 2024 13:47:42 -0400 Subject: [PATCH] [SW-1539] don't publish images from hand camera if gripperless param set (#518) ## Change Overview The `gripperless` parameter was added recently to the python spot_ros2.py node . This also adds support for the same parameter in the C++ nodes, namely the image publishing stack so it doesn't try to request images from the hand camera if the robot is gripperless. ## Testing Done - [x] Ran image publishers on a robot with an arm and the gripperless param set to `True` (since I didn't have access to a robot with the gripperless firmware currently). Hand camera does not get published to. - [x] new CI tests pass --- .../interfaces/parameter_interface_base.hpp | 11 ++- .../interfaces/rclcpp_parameter_interface.hpp | 6 +- .../spot_driver/launch/spot_launch_helpers.py | 10 ++- .../src/images/spot_image_publisher.cpp | 5 +- .../interfaces/rclcpp_parameter_interface.cpp | 18 +++- .../fake/fake_parameter_interface.hpp | 12 ++- .../test/src/test_parameter_interface.cpp | 89 +++++++++++++++++-- 7 files changed, 123 insertions(+), 28 deletions(-) diff --git a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp index e11fdd360..4a837bb2e 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/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 getDefaultCamerasUsed(bool has_arm) const = 0; - virtual tl::expected, std::string> getCamerasUsed(bool has_arm) const = 0; + virtual bool getGripperless() const = 0; + virtual std::set getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0; + virtual tl::expected, 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 diff --git a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp index 1d86f8f7f..0a205ac49 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/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 getDefaultCamerasUsed(const bool has_arm) const override; + [[nodiscard]] bool getGripperless() const override; + [[nodiscard]] std::set getDefaultCamerasUsed(const bool has_arm, + const bool gripperless) const override; [[nodiscard]] tl::expected, std::string> getCamerasUsed( - const bool has_arm) const override; + const bool has_arm, const bool gripperless) const override; private: std::shared_ptr node_; diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 50914467b..124702eb6 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/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): diff --git a/spot_driver/src/images/spot_image_publisher.cpp b/spot_driver/src/images/spot_image_publisher.cpp index 2f885e2dc..9bc7675f6 100644 --- a/spot_driver/src/images/spot_image_publisher.cpp +++ b/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 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 diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 117d41d98..1eaa024ba 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/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(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); } -std::set RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm) const { - const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm; +bool RclcppParameterInterface::getGripperless() const { + return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); +} + +std::set 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_cameras_used; for (const auto& camera : kDefaultCamerasUsed) { spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera))); @@ -196,8 +203,9 @@ std::set RclcppParameterInterface::getDefaultCamerasUsed( } tl::expected, 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 kDefaultCamerasUsedVector(std::begin(kDefaultCamerasUsed), std::end(kDefaultCamerasUsed)); const auto cameras_used_param = @@ -208,6 +216,8 @@ tl::expected, 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) { diff --git a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 32af0efee..433db5c0a 100644 --- a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp +++ b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp @@ -40,8 +40,10 @@ class FakeParameterInterface : public ParameterInterfaceBase { std::string getSpotName() const override { return spot_name; } - std::set getDefaultCamerasUsed(const bool has_arm) const override { - const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm; + bool getGripperless() const override { return gripperless; } + + std::set getDefaultCamerasUsed(const bool has_arm, const bool gripperless) const override { + const auto kDefaultCamerasUsed = (has_arm && !gripperless) ? kCamerasWithHand : kCamerasWithoutHand; std::set spot_cameras_used; for (const auto& camera : kDefaultCamerasUsed) { spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera))); @@ -49,8 +51,9 @@ class FakeParameterInterface : public ParameterInterfaceBase { return spot_cameras_used; } - tl::expected, std::string> getCamerasUsed(const bool has_arm) const override { - return getDefaultCamerasUsed(has_arm); + tl::expected, 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 diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index b615f8926..74546d22a 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/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 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