Skip to content

Commit

Permalink
Add missing definition for CameraPublisher::publish overload (#278)
Browse files Browse the repository at this point in the history
  • Loading branch information
s-hall authored Jan 19, 2024
1 parent c86efc3 commit 27dd6e0
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 0 deletions.
19 changes: 19 additions & 0 deletions image_transport/src/camera_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,25 @@ void CameraPublisher::publish(
impl_->info_pub_->publish(*info);
}

void CameraPublisher::publish(
sensor_msgs::msg::Image & image, sensor_msgs::msg::CameraInfo & info,
rclcpp::Time stamp) const
{
if (!impl_ || !impl_->isValid()) {
// TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged
auto logger = impl_ ? impl_->logger_ : rclcpp::get_logger("image_transport");
RCLCPP_FATAL(
logger,
"Call to publish() on an invalid image_transport::CameraPublisher");
return;
}

image.header.stamp = stamp;
info.header.stamp = stamp;
impl_->image_pub_.publish(image);
impl_->info_pub_->publish(info);
}

void CameraPublisher::shutdown()
{
if (impl_) {
Expand Down
3 changes: 3 additions & 0 deletions image_transport/test/test_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ TEST_F(TestPublisher, camera_publisher) {
camera_pub.publish(
sensor_msgs::msg::Image::ConstSharedPtr(),
sensor_msgs::msg::CameraInfo::ConstSharedPtr());
sensor_msgs::msg::Image image;
sensor_msgs::msg::CameraInfo info;
camera_pub.publish(image, info, rclcpp::Time());
}

TEST_F(TestPublisher, image_transport_camera_publisher) {
Expand Down

0 comments on commit 27dd6e0

Please sign in to comment.