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

Use image_transport::CameraPublisher to publish synchronized Image and CameraInfo messages #180

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Use image_transport::CameraPublisher to publish synchronized Image and CameraInfo messages #180

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ae80926
add dependency on image_transport package
jschornak-bdai Nov 2, 2023
46ddf33
replace separate Image and CameraInfo publishers with combined image_…
jschornak-bdai Nov 2, 2023
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
1 change: 1 addition & 0 deletions spot_driver_cpp/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS
cv_bridge
geometry_msgs
image_transport
rclcpp
rclcpp_components
sensor_msgs
Expand Down
10 changes: 6 additions & 4 deletions spot_driver_cpp/include/spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

#pragma once

#include <image_transport/camera_publisher.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/node.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <spot_driver_cpp/interfaces/publisher_interface_base.hpp>
Expand Down Expand Up @@ -42,10 +44,10 @@ class RclcppPublisherInterface : public PublisherInterfaceBase {
/** @brief rclcpp node use to create the publishers. */
std::shared_ptr<rclcpp::Node> node_;

/** @brief Map between image topic names and image publishers. */
std::unordered_map<std::string, std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> image_publishers_;
/** @brief ImageTransport instance used to create CameraPublishers. */
image_transport::ImageTransport image_transport_;

/** @brief Map between camera info topic names and camera info publishers. */
std::unordered_map<std::string, std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>>> info_publishers_;
/** @brief Map between image topic names and camera publishers. */
std::unordered_map<std::string, image_transport::CameraPublisher> publishers_;
};
} // namespace spot_ros2
1 change: 1 addition & 0 deletions spot_driver_cpp/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>

<depend>bosdyn</depend>
<depend>image_transport</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>geometry_msgs</depend>
Expand Down
35 changes: 8 additions & 27 deletions spot_driver_cpp/src/interfaces/rclcpp_publisher_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,60 +1,41 @@
// Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved.

#include <rclcpp/qos.hpp>
#include <spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp>
#include <tl_expected/expected.hpp>

namespace {
constexpr auto kPublisherHistoryDepth = 1;

constexpr auto kImageTopicSuffix = "image";
constexpr auto kCameraInfoTopicSuffix = "camera_info";
} // namespace

namespace spot_ros2 {

RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr<rclcpp::Node>& node) : node_{node} {}
RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr<rclcpp::Node>& node)
: node_{node}, image_transport_{node_} {}

void RclcppPublisherInterface::createPublishers(const std::set<ImageSource>& image_sources) {
image_publishers_.clear();
info_publishers_.clear();
publishers_.clear();

for (const auto& image_source : image_sources) {
// Since these topic names do not have a leading `/` character, they will be published within the namespace of the
// node, which should match the name of the robot. For example, the topic for the front left RGB camera will
// ultimately appear as `/MyRobotName/camera/frontleft/image`.
const auto topic_name_base = toRosTopic(image_source);

const auto image_topic_name = topic_name_base + "/" + kImageTopicSuffix;

image_publishers_.try_emplace(image_topic_name,
node_->create_publisher<sensor_msgs::msg::Image>(
image_topic_name, rclcpp::QoS(rclcpp::KeepLast(kPublisherHistoryDepth))));

const auto info_topic_name = topic_name_base + "/" + kCameraInfoTopicSuffix;
info_publishers_.try_emplace(info_topic_name,
node_->create_publisher<sensor_msgs::msg::CameraInfo>(
info_topic_name, rclcpp::QoS(rclcpp::KeepLast(kPublisherHistoryDepth))));
const auto image_topic_name = toRosTopic(image_source) + "/" + kImageTopicSuffix;
publishers_.try_emplace(image_topic_name,
image_transport_.advertiseCamera(image_topic_name, kPublisherHistoryDepth));
}
}

tl::expected<void, std::string> RclcppPublisherInterface::publish(
const std::map<ImageSource, ImageWithCameraInfo>& images) {
for (const auto& [image_source, image_data] : images) {
const auto topic_name_base = toRosTopic(image_source);
const auto image_topic_name = topic_name_base + "/" + kImageTopicSuffix;
const auto info_topic_name = topic_name_base + "/" + kCameraInfoTopicSuffix;
const auto image_topic_name = toRosTopic(image_source) + "/" + kImageTopicSuffix;

try {
image_publishers_.at(image_topic_name)->publish(image_data.image);
publishers_.at(image_topic_name).publish(image_data.image, image_data.info);
} catch (const std::out_of_range& e) {
return tl::make_unexpected("No publisher exists for image topic `" + image_topic_name + "`.");
}
try {
info_publishers_.at(info_topic_name)->publish(image_data.info);
} catch (const std::out_of_range& e) {
return tl::make_unexpected("No publisher exists for camera info topic`" + info_topic_name + "`.");
}
}

return {};
Expand Down