Skip to content

Commit

Permalink
[SW-1539] don't publish images from hand camera if gripperless param …
Browse files Browse the repository at this point in the history 
…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
  • Loading branch information
@khughes-bdai
khughes-bdai authored Nov 1, 2024
1 parent e3992eb commit feea7b6
Show file tree
Hide file tree
Showing 7 changed files with 123 additions and 28 deletions.
11 changes: 7 additions & 4 deletions spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
6 changes: 4 additions & 2 deletions spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
10 changes: 7 additions & 3 deletions spot_driver/spot_driver/launch/spot_launch_helpers.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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):
Expand Down
5 changes: 3 additions & 2 deletions spot_driver/src/images/spot_image_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 14 additions & 4 deletions spot_driver/src/interfaces/rclcpp_parameter_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)));
Expand All @@ -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 =
Expand All @@ -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) {
Expand Down
12 changes: 8 additions & 4 deletions spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"};
Expand All @@ -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
89 changes: 80 additions & 9 deletions spot_driver/test/src/test_parameter_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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));
Expand All @@ -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));
}
Expand All @@ -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!"));
}
Expand All @@ -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

0 comments on commit feea7b6

Please sign in to comment.