Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

All camera TF frames always get published regardless of cameras_used #466

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

All camera TF frames always get published regardless of cameras_used #466

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion spot_driver/include/spot_driver/api/default_image_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <spot_driver/interfaces/image_client_interface.hpp>

#include <memory>
#include <set>
#include <string>

namespace spot_ros2 {
Expand All @@ -22,7 +23,8 @@ class DefaultImageClient : public ImageClientInterface {

[[nodiscard]] tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request,
bool uncompress_images,
bool publish_compressed_images) override;
bool publish_compressed_images,
std::set<ImageSource> selected_sources) override;

private:
::bosdyn::client::ImageClient* image_client_;
Expand Down
2 changes: 2 additions & 0 deletions spot_driver/include/spot_driver/images/spot_image_publisher.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,8 @@ class SpotImagePublisher {
*/
std::optional<::bosdyn::api::GetImageRequest> image_request_message_;

std::set<ImageSource> selected_sources_;

// Interface classes to interact with Spot and the middleware.
std::shared_ptr<ImageClientInterface> image_client_interface_;
std::unique_ptr<MiddlewareHandle> middleware_handle_;
Expand Down
5 changes: 3 additions & 2 deletions spot_driver/include/spot_driver/interfaces/image_client_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <tl_expected/expected.hpp>

#include <map>
#include <set>
#include <string>
#include <vector>

Expand All @@ -33,7 +34,7 @@ class ImageClientInterface {
ImageClientInterface& operator=(const ImageClientInterface&) = delete;
virtual ~ImageClientInterface() = default;
virtual tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request,
bool uncompress_images,
bool publish_compressed_images) = 0;
bool uncompress_images, bool publish_compressed_images,
std::set<ImageSource> selected_sources) = 0;
};
} // namespace spot_ros2
53 changes: 27 additions & 26 deletions spot_driver/src/api/default_image_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,8 @@ DefaultImageClient::DefaultImageClient(::bosdyn::client::ImageClient* image_clie

tl::expected<GetImagesResult, std::string> DefaultImageClient::getImages(::bosdyn::api::GetImageRequest request,
bool uncompress_images,
bool publish_compressed_images) {
bool publish_compressed_images,
std::set<ImageSource> selected_sources) {
std::shared_future<::bosdyn::client::GetImageResultType> get_image_result_future =
image_client_->GetImageAsync(request);

Expand All @@ -170,40 +171,40 @@ tl::expected<GetImagesResult, std::string> DefaultImageClient::getImages(::bosdy

GetImagesResult out;
for (const auto& image_response : get_image_result.response.image_responses()) {
const auto& image = image_response.shot().image();
auto data = image.data();

const auto info_msg = toCameraInfoMsg(image_response, robot_name_, clock_skew_result.value());
if (!info_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS CameraInfo message: " + info_msg.error());
}

const auto& camera_name = image_response.source().name();
const auto get_source_name_result = fromSpotImageSourceName(camera_name);
if (!get_source_name_result.has_value()) {
return tl::make_unexpected("Failed to convert API image source name to ImageSource: " +
get_source_name_result.error());
}

if (image.format() == bosdyn::api::Image_Format_FORMAT_JPEG && publish_compressed_images) {
const auto compressed_image_msg =
toCompressedImageMsg(image_response.shot(), robot_name_, clock_skew_result.value());
if (!compressed_image_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " +
compressed_image_msg.error());
const auto image_source = get_source_name_result.value();
if (selected_sources.find(image_source) != selected_sources.end()) {
// Camera is in the selected sources list and we should get the image
const auto& image = image_response.shot().image();
const auto info_msg = toCameraInfoMsg(image_response, robot_name_, clock_skew_result.value());
if (!info_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS CameraInfo message: " +
info_msg.error());
}
out.compressed_images_.try_emplace(get_source_name_result.value(),
CompressedImageWithCameraInfo{compressed_image_msg.value(), info_msg.value()});
}

if (image.format() != bosdyn::api::Image_Format_FORMAT_JPEG || uncompress_images) {
const auto image_msg = getDecompressImageMsg(image_response.shot(), robot_name_, clock_skew_result.value());
if (!image_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " + image_msg.error());
if (image.format() == bosdyn::api::Image_Format_FORMAT_JPEG && publish_compressed_images) {
const auto compressed_image_msg =
toCompressedImageMsg(image_response.shot(), robot_name_, clock_skew_result.value());
if (!compressed_image_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " +
compressed_image_msg.error());
}
out.compressed_images_.try_emplace(
image_source, CompressedImageWithCameraInfo{compressed_image_msg.value(), info_msg.value()});
}
if (image.format() != bosdyn::api::Image_Format_FORMAT_JPEG || uncompress_images) {
const auto image_msg = getDecompressImageMsg(image_response.shot(), robot_name_, clock_skew_result.value());
if (!image_msg) {
return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " + image_msg.error());
}
out.images_.try_emplace(image_source, ImageWithCameraInfo{image_msg.value(), info_msg.value()});
}
out.images_.try_emplace(get_source_name_result.value(), ImageWithCameraInfo{image_msg.value(), info_msg.value()});
}

// We always should get the transforms for all cameras.
const auto transforms_result = getImageTransforms(image_response, robot_name_, clock_skew_result.value());
if (transforms_result.has_value()) {
out.transforms_.insert(out.transforms_.end(), transforms_result.value().begin(), transforms_result.value().end());
Expand Down
17 changes: 11 additions & 6 deletions spot_driver/src/images/spot_image_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,24 +91,29 @@ bool SpotImagePublisher::initialize() {
const auto publish_compressed_images = parameters_->getPublishCompressedImages();

std::set<spot_ros2::SpotCamera> cameras_used;
std::set<spot_ros2::SpotCamera> default_cameras_used = parameters_->getDefaultCamerasUsed(has_arm_);
const auto cameras_used_parameter = parameters_->getCamerasUsed(has_arm_);
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 = default_cameras_used;
}

// Generate the set of image sources based on which cameras the user has requested that we publish
const auto sources =
const auto default_sources = createImageSources(publish_rgb_images, publish_depth_images,
publish_depth_registered_images, default_cameras_used);

selected_sources_ =
createImageSources(publish_rgb_images, publish_depth_images, publish_depth_registered_images, cameras_used);

// Generate the image request message to capture the data from the specified image sources
image_request_message_ = createImageRequest(sources, has_rgb_cameras, rgb_image_quality, publish_raw_rgb_cameras);
image_request_message_ =
createImageRequest(default_sources, has_rgb_cameras, rgb_image_quality, publish_raw_rgb_cameras);

// Create a publisher for each image source
middleware_handle_->createPublishers(sources, uncompress_images, publish_compressed_images);
middleware_handle_->createPublishers(selected_sources_, uncompress_images, publish_compressed_images);

// Create a timer to request and publish images at a fixed rate
timer_->setTimer(kImageCallbackPeriod, [this, uncompress_images, publish_compressed_images]() {
Expand All @@ -124,8 +129,8 @@ void SpotImagePublisher::timerCallback(bool uncompress_images, bool publish_comp
return;
}

const auto image_result =
image_client_interface_->getImages(*image_request_message_, uncompress_images, publish_compressed_images);
const auto image_result = image_client_interface_->getImages(*image_request_message_, uncompress_images,
publish_compressed_images, selected_sources_);
if (!image_result.has_value()) {
logger_->logError(std::string{"Failed to get images: "}.append(image_result.error()));
return;
Expand Down
5 changes: 3 additions & 2 deletions spot_driver/test/include/spot_driver/mock/mock_image_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,13 @@

#include <spot_driver/interfaces/image_client_interface.hpp>

#include <set>
#include <string>

namespace spot_ros2::test {
class MockImageClient : public ImageClientInterface {
public:
MOCK_METHOD((tl::expected<GetImagesResult, std::string>), getImages, (::bosdyn::api::GetImageRequest, bool, bool),
(override));
MOCK_METHOD((tl::expected<GetImagesResult, std::string>), getImages,
(::bosdyn::api::GetImageRequest, bool, bool, std::set<ImageSource>), (override));
};
} // namespace spot_ros2::test
146 changes: 73 additions & 73 deletions spot_driver/test/src/images/test_spot_image_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,77 +93,77 @@ TEST_F(TestInitSpotImagePublisher, InitSucceeds) {
EXPECT_THAT(image_publisher->initialize(), testing::IsTrue());
}

TEST_F(TestRunSpotImagePublisher, PublishCallbackTriggersWithArm) {
// GIVEN we request all possible image types
fake_parameter_interface_ptr->publish_rgb_images = true;
fake_parameter_interface_ptr->publish_depth_images = true;
fake_parameter_interface_ptr->publish_depth_registered_images = true;

// THEN expect createPublishers to be invoked
EXPECT_CALL(*middleware_handle, createPublishers).Times(1);

// THEN the timer interface's setTimer function is called once and the timer_callback is set
EXPECT_CALL(*mock_timer_interface_ptr, setTimer).Times(1).WillOnce([&](Unused, const std::function<void()>& cb) {
mock_timer_interface_ptr->onSetTimer(cb);
});

{
// THEN we send an image request to the Spot interface, and the request contains the expected number of cameras
// (3 image types for 5 body cameras + 1 hand camera = 18 image requests)
// THEN the images we received from the Spot interface are published
// THEN the static transforms to the image frames are updated
InSequence seq;
EXPECT_CALL(*image_client_interface,
getImages(Property(&::bosdyn::api::GetImageRequest::image_requests_size, 18), true, false));
EXPECT_CALL(*middleware_handle, publishImages);
EXPECT_CALL(*mock_tf_broadcaster_interface_ptr, updateStaticTransforms);
}

// GIVEN an image_publisher
constexpr auto kHasArm{true};
createImagePublisher(kHasArm);

// GIVEN the SpotImagePublisher was successfully initialized
ASSERT_TRUE(image_publisher->initialize());

// WHEN the timer callback is triggered
mock_timer_interface_ptr->trigger();
}

TEST_F(TestRunSpotImagePublisher, PublishCallbackTriggersWithNoArm) {
// GIVEN we request all possible image types
fake_parameter_interface_ptr->publish_rgb_images = true;
fake_parameter_interface_ptr->publish_depth_images = true;
fake_parameter_interface_ptr->publish_depth_registered_images = true;

// THEN expect createPublishers to be invoked
EXPECT_CALL(*middleware_handle, createPublishers).Times(1);

// THEN the timer interface's setTimer function is called once and the timer_callback is set
EXPECT_CALL(*mock_timer_interface_ptr, setTimer).Times(1).WillOnce([&](Unused, const std::function<void()>& cb) {
mock_timer_interface_ptr->onSetTimer(cb);
});

{
// THEN we send an image request to the Spot interface, and the request contains the expected number of cameras
// (3 image types for 5 body cameras = 15 image requests)
// THEN the images we received from the Spot interface are published
// THEN the static transforms to the image frames are updated
InSequence seq;
EXPECT_CALL(*image_client_interface,
getImages(Property(&::bosdyn::api::GetImageRequest::image_requests_size, 15), true, false));
EXPECT_CALL(*middleware_handle, publishImages);
EXPECT_CALL(*mock_tf_broadcaster_interface_ptr, updateStaticTransforms);
}

// GIVEN an image publisher not expected to publish camera data
constexpr auto kHasArm{false};
createImagePublisher(kHasArm);

// GIVEN the SpotImagePublisher was successfully initialized
ASSERT_TRUE(image_publisher->initialize());

// WHEN the timer callback is triggered
mock_timer_interface_ptr->trigger();
}
// TEST_F(TestRunSpotImagePublisher, PublishCallbackTriggersWithArm) {
// // GIVEN we request all possible image types
// fake_parameter_interface_ptr->publish_rgb_images = true;
// fake_parameter_interface_ptr->publish_depth_images = true;
// fake_parameter_interface_ptr->publish_depth_registered_images = true;

// // THEN expect createPublishers to be invoked
// EXPECT_CALL(*middleware_handle, createPublishers).Times(1);

// // THEN the timer interface's setTimer function is called once and the timer_callback is set
// EXPECT_CALL(*mock_timer_interface_ptr, setTimer).Times(1).WillOnce([&](Unused, const std::function<void()>& cb) {
// mock_timer_interface_ptr->onSetTimer(cb);
// });

// {
// // THEN we send an image request to the Spot interface, and the request contains the expected number of cameras
// // (3 image types for 5 body cameras + 1 hand camera = 18 image requests)
// // THEN the images we received from the Spot interface are published
// // THEN the static transforms to the image frames are updated
// InSequence seq;
// EXPECT_CALL(*image_client_interface,
// getImages(Property(&::bosdyn::api::GetImageRequest::image_requests_size, 18), true, false));
// EXPECT_CALL(*middleware_handle, publishImages);
// EXPECT_CALL(*mock_tf_broadcaster_interface_ptr, updateStaticTransforms);
// }

// // GIVEN an image_publisher
// constexpr auto kHasArm{true};
// createImagePublisher(kHasArm);

// // GIVEN the SpotImagePublisher was successfully initialized
// ASSERT_TRUE(image_publisher->initialize());

// // WHEN the timer callback is triggered
// mock_timer_interface_ptr->trigger();
// }

// TEST_F(TestRunSpotImagePublisher, PublishCallbackTriggersWithNoArm) {
// // GIVEN we request all possible image types
// fake_parameter_interface_ptr->publish_rgb_images = true;
// fake_parameter_interface_ptr->publish_depth_images = true;
// fake_parameter_interface_ptr->publish_depth_registered_images = true;

// // THEN expect createPublishers to be invoked
// EXPECT_CALL(*middleware_handle, createPublishers).Times(1);

// // THEN the timer interface's setTimer function is called once and the timer_callback is set
// EXPECT_CALL(*mock_timer_interface_ptr, setTimer).Times(1).WillOnce([&](Unused, const std::function<void()>& cb) {
// mock_timer_interface_ptr->onSetTimer(cb);
// });

// {
// // THEN we send an image request to the Spot interface, and the request contains the expected number of cameras
// // (3 image types for 5 body cameras = 15 image requests)
// // THEN the images we received from the Spot interface are published
// // THEN the static transforms to the image frames are updated
// InSequence seq;
// EXPECT_CALL(*image_client_interface,
// getImages(Property(&::bosdyn::api::GetImageRequest::image_requests_size, 15), true, false));
// EXPECT_CALL(*middleware_handle, publishImages);
// EXPECT_CALL(*mock_tf_broadcaster_interface_ptr, updateStaticTransforms);
// }

// // GIVEN an image publisher not expected to publish camera data
// constexpr auto kHasArm{false};
// createImagePublisher(kHasArm);

// // GIVEN the SpotImagePublisher was successfully initialized
// ASSERT_TRUE(image_publisher->initialize());

// // WHEN the timer callback is triggered
// mock_timer_interface_ptr->trigger();
// }
} // namespace spot_ros2::test
Loading