diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst index f6054d6f2..60bd4b8b7 100644 --- a/camera_calibration/CHANGELOG.rst +++ b/camera_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package camera_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ + 6.0.5 (2024-10-30) ------------------ diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml index 330cda522..afb306b31 100644 --- a/camera_calibration/package.xml +++ b/camera_calibration/package.xml @@ -2,7 +2,7 @@ camera_calibration - 6.0.5 + 6.0.6 camera_calibration allows easy calibration of monocular or stereo cameras using a checkerboard calibration target. diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py index 401ea9fa6..e87716237 100644 --- a/camera_calibration/setup.py +++ b/camera_calibration/setup.py @@ -5,7 +5,7 @@ setup( name=PACKAGE_NAME, - version='6.0.5', + version='6.0.6', packages=["camera_calibration", "camera_calibration.nodes"], data_files=[ ('share/ament_index/resource_index/packages', diff --git a/depth_image_proc/CHANGELOG.rst b/depth_image_proc/CHANGELOG.rst index 1716878c7..fdf48fa9b 100644 --- a/depth_image_proc/CHANGELOG.rst +++ b/depth_image_proc/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package depth_image_proc ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ +* Support QoS override parameters in depth_image_proc/register (`#1043 `_) + This PR adds support to the `depth_image_proc` - `register` node for + setting External QoS Configuration on topic _subscriptions\_. +* Contributors: Stuart Alldritt + 6.0.5 (2024-10-30) ------------------ diff --git a/depth_image_proc/package.xml b/depth_image_proc/package.xml index 3092553ac..428ae6533 100644 --- a/depth_image_proc/package.xml +++ b/depth_image_proc/package.xml @@ -2,7 +2,7 @@ depth_image_proc - 6.0.5 + 6.0.6 Contains components for processing depth images such as those diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 7db288436..690d28f07 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -133,14 +133,19 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) sub_depth_info_.unsubscribe(); sub_rgb_info_.unsubscribe(); } else if (!sub_depth_image_.getSubscriber()) { + // Allow overriding QoS settings (history, depth, reliability) + rclcpp::SubscriptionOptions sub_options; + sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + // For compressed topics to remap appropriately, we need to pass a // fully expanded and remapped topic name to image_transport auto node_base = this->get_node_base_interface(); std::string topic = node_base->resolve_topic_or_service_name("depth/image_rect", false); image_transport::TransportHints hints(this, "raw", "depth_image_transport"); - sub_depth_image_.subscribe(this, topic, hints.getTransport()); - sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10)); - sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10)); + sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default, + sub_options); + sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10), sub_options); + sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10), sub_options); } }; // For compressed topics to remap appropriately, we need to pass a diff --git a/image_pipeline/CHANGELOG.rst b/image_pipeline/CHANGELOG.rst index 3b56167f1..93b086ab5 100644 --- a/image_pipeline/CHANGELOG.rst +++ b/image_pipeline/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package image_pipeline ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ + 6.0.5 (2024-10-30) ------------------ diff --git a/image_pipeline/package.xml b/image_pipeline/package.xml index 2ec7beb1a..573273553 100644 --- a/image_pipeline/package.xml +++ b/image_pipeline/package.xml @@ -2,7 +2,7 @@ image_pipeline - 6.0.5 + 6.0.6 image_pipeline fills the gap between getting raw images from a camera driver and higher-level vision processing. Vincent Rabaud diff --git a/image_proc/CHANGELOG.rst b/image_proc/CHANGELOG.rst index e2dc58ad8..d6243d510 100644 --- a/image_proc/CHANGELOG.rst +++ b/image_proc/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package image_proc ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ + 6.0.5 (2024-10-30) ------------------ * Use TF2 package for quaternion conversion (`#1031 `_) diff --git a/image_proc/package.xml b/image_proc/package.xml index 70debc4fc..3720c969c 100644 --- a/image_proc/package.xml +++ b/image_proc/package.xml @@ -2,7 +2,7 @@ image_proc - 6.0.5 + 6.0.6 Single image rectification and color processing. Vincent Rabaud diff --git a/image_publisher/CHANGELOG.rst b/image_publisher/CHANGELOG.rst index daad6c57a..a41f6873d 100644 --- a/image_publisher/CHANGELOG.rst +++ b/image_publisher/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package image_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ + 6.0.5 (2024-10-30) ------------------ diff --git a/image_publisher/package.xml b/image_publisher/package.xml index 38c5dc53c..a621501fe 100644 --- a/image_publisher/package.xml +++ b/image_publisher/package.xml @@ -2,7 +2,7 @@ image_publisher - 6.0.5 + 6.0.6 Contains a node publish an image stream from single image file diff --git a/image_rotate/CHANGELOG.rst b/image_rotate/CHANGELOG.rst index e4510758e..22739e5bc 100644 --- a/image_rotate/CHANGELOG.rst +++ b/image_rotate/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package image_rotate ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ +* add image_flip node (`#942 `_) + This is a continuation of + https://github.com/ros-perception/image_pipeline/pull/756: + * [x] Squashed 16 commits in original PR for ease of rebase/review + * [x] Moved node into image_rotate package + * [x] Added lazy subscriber + * [x] Removes QoS parameters - will add proper QoS overrides in a + different PR (when we do the same for image_rotate) + * [x] Adds documentation + --------- + Co-authored-by: David Conner + Co-authored-by: Alejandro Hernández Cordero +* Contributors: Michael Ferguson + 6.0.5 (2024-10-30) ------------------ diff --git a/image_rotate/CMakeLists.txt b/image_rotate/CMakeLists.txt index 49c3b94fc..4e1720900 100644 --- a/image_rotate/CMakeLists.txt +++ b/image_rotate/CMakeLists.txt @@ -24,6 +24,13 @@ ament_auto_add_executable(image_rotate_bin src/image_rotate.cpp) set_target_properties(image_rotate_bin PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) target_link_libraries(image_rotate_bin ${OpenCV_LIBRARIES} ${PROJECT_NAME}) +ament_auto_add_library(image_flip SHARED src/image_flip.cpp) +target_link_libraries(image_flip ${OpenCV_LIBRARIES}) +rclcpp_components_register_node(image_flip + PLUGIN "${PROJECT_NAME}::ImageFlipNode" + EXECUTABLE image_flip_node +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/image_rotate/doc/components.rst b/image_rotate/doc/components.rst index cb6b77273..daa9c1918 100644 --- a/image_rotate/doc/components.rst +++ b/image_rotate/doc/components.rst @@ -1,6 +1,36 @@ Nodes and Components ==================== +image_rotate::ImageFlipNode +----------------------------- +This is a simplified version image rotate which merely rotates/flips +the images. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **image** (sensor_msgs/Image): Image to be rotated. + * **camera_info** (sensor_msgs/CameraInfo): Camera metadata, only + used if ``use_camera_info`` is set to true. + +Published Topics +^^^^^^^^^^^^^^^^ + * **rotated/image** (sensor_msgs/Image): Rotated image. + * **out/camera_info** (sensor_msgs/CameraInfo): Camera metadata, with binning and + ROI fields adjusted to match output raw image. + +Parameters +^^^^^^^^^^ + * **output_frame_id** (str, default: ""): Frame to publish for the image's + new orientation. Empty means add '_rotated' suffix to the image frame. + * **use_camera_info** (bool, default: True): Indicates that the camera_info + topic should be subscribed to to get the default input_frame_id. + Otherwise the frame from the image message will be used. + * **rotation_steps** (int, default: 2): Number of times to rotate the image: + + * 1 is transpose and flip in CCW + * 2 is flip (180 mirroring) + * 3 is transpose and flip in CW + image_rotate::ImageRotateNode ----------------------------- Node to rotate an image for visualization. The node takes a source diff --git a/image_rotate/include/image_rotate/image_flip.hpp b/image_rotate/include/image_rotate/image_flip.hpp new file mode 100644 index 000000000..7285567d2 --- /dev/null +++ b/image_rotate/include/image_rotate/image_flip.hpp @@ -0,0 +1,103 @@ +// Copyright (c) 2022, CHRISLab, Christopher Newport University +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef IMAGE_ROTATE__IMAGE_FLIP_HPP_ +#define IMAGE_ROTATE__IMAGE_FLIP_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "image_rotate/visibility.h" + +namespace image_rotate +{ + +struct ImageFlipConfig +{ + std::string output_frame_id; + int rotation_steps; + bool use_camera_info; +}; + +class ImageFlipNode : public rclcpp::Node +{ +public: + IMAGE_ROTATE_PUBLIC ImageFlipNode(rclcpp::NodeOptions options); + +private: + const std::string frameWithDefault(const std::string & frame, const std::string & image_frame); + void imageCallbackWithInfo( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info); + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + void do_work( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info, + const std::string input_frame_from_msg); + void onInit(); + + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_sub_; + std::shared_ptr tf_pub_; + bool tf_unpublished_; + + ImageFlipConfig config_; + + // Subscriber - only one is used at a time - depends on use_camera_info + image_transport::Subscriber img_sub_; + image_transport::CameraSubscriber cam_sub_; + + // Publisher - only one is used at a time - depends on use_camera_info + image_transport::Publisher img_pub_; + image_transport::CameraPublisher cam_pub_; + + double angle_; + tf2::TimePoint prev_stamp_; + geometry_msgs::msg::TransformStamped transform_; +}; +} // namespace image_rotate + +#endif // IMAGE_ROTATE__IMAGE_FLIP_HPP_ diff --git a/image_rotate/include/image_rotate/image_rotate_node.hpp b/image_rotate/include/image_rotate/image_rotate_node.hpp index 67e08eae3..b65c189a7 100644 --- a/image_rotate/include/image_rotate/image_rotate_node.hpp +++ b/image_rotate/include/image_rotate/image_rotate_node.hpp @@ -93,13 +93,14 @@ class ImageRotateNode : public rclcpp::Node image_rotate::ImageRotateConfig config_; image_transport::Publisher img_pub_; + + // Subscriber - only one is used at a time - depends on use_camera_info image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; geometry_msgs::msg::Vector3Stamped target_vector_; geometry_msgs::msg::Vector3Stamped source_vector_; - int subscriber_count_; double angle_; tf2::TimePoint prev_stamp_; }; diff --git a/image_rotate/launch/image_flip.launch.py b/image_rotate/launch/image_flip.launch.py new file mode 100644 index 000000000..be0930535 --- /dev/null +++ b/image_rotate/launch/image_flip.launch.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022, CHRISLab, Christopher Newport University +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Demonstration of basic launch of the image_flip_node with remappings.""" + +from launch import LaunchDescription +import launch_ros.actions + + +def generate_launch_description(): + """Launch description for basic launch of the image_flip.""" + return LaunchDescription([ + launch_ros.actions.Node( + package='image_rotate', executable='image_flip', + output='screen', name='camera_flip', + remappings=[('image', 'camera/rgb/image_raw'), + ('rotated/image', 'camera_rotated/image_rotated')], + parameters=[{'output_frame_id': 'camera_rotated', + 'rotation_steps': 2, + 'use_camera_info': True}])]) diff --git a/image_rotate/package.xml b/image_rotate/package.xml index d0e84b250..1cd60aa2e 100644 --- a/image_rotate/package.xml +++ b/image_rotate/package.xml @@ -2,7 +2,7 @@ image_rotate - 6.0.5 + 6.0.6

Contains a node that rotates an image stream in a way that minimizes @@ -34,6 +34,7 @@ https://github.com/ros-perception/image_pipeline/issues https://github.com/ros-perception/image_pipeline Blaise Gassend + David Conner ament_cmake_auto diff --git a/image_rotate/src/image_flip.cpp b/image_rotate/src/image_flip.cpp new file mode 100644 index 000000000..3070db651 --- /dev/null +++ b/image_rotate/src/image_flip.cpp @@ -0,0 +1,261 @@ +// Copyright (c) 2022, CHRISLab, Christopher Newport University +// Copyright (c) 2014, JSK Lab. +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "image_rotate/image_flip.hpp" + +#include +#include +#include +#include + +namespace image_rotate +{ + +ImageFlipNode::ImageFlipNode(rclcpp::NodeOptions options) +: Node("ImageFlipNode", options) +{ + config_.output_frame_id = this->declare_parameter("output_frame_id", std::string("")); + config_.rotation_steps = this->declare_parameter("rotation_steps", 2); + config_.use_camera_info = this->declare_parameter("use_camera_info", true); + + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); + + auto reconfigureCallback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + if (parameter.get_name() == "output_frame_id") { + config_.output_frame_id = parameter.as_string(); + RCLCPP_INFO(get_logger(), "Reset output_frame_id '%s'", config_.output_frame_id.c_str()); + } else if (parameter.get_name() == "rotation_steps") { + config_.rotation_steps = parameter.as_int(); + angle_ = config_.rotation_steps * M_PI / 2.0; + RCLCPP_INFO(get_logger(), "Reset rotation_steps as '%d'", config_.rotation_steps); + transform_.transform.rotation = + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_)); + tf_unpublished_ = true; + } + } + return result; + }; + on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(reconfigureCallback); + onInit(); + angle_ = config_.rotation_steps * M_PI / 2.0; + transform_.transform.translation.x = 0; + transform_.transform.translation.y = 0; + transform_.transform.translation.z = 0; + transform_.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_)); +} + +const std::string ImageFlipNode::frameWithDefault( + const std::string & frame, + const std::string & image_frame) +{ + if (frame.empty()) { + return image_frame; + } + return frame; +} + +void ImageFlipNode::imageCallbackWithInfo( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + std::string frame = cam_info->header.frame_id; + if (frame.empty()) { + frame = msg->header.frame_id; + } + do_work(msg, cam_info, frame); +} + +void ImageFlipNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + do_work(msg, nullptr, msg->header.frame_id); +} + +void ImageFlipNode::do_work( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info, + const std::string input_frame_from_msg) +{ + // Transform the image. + try { + // Convert the image into something opencv can handle. + cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image; + cv::Mat out_image; + + // based on https://stackoverflow.com/questions/15043152/rotate-opencv-matrix-by-90-180-270-degrees + if (config_.rotation_steps == 1) { + transpose(in_image, out_image); + flip(out_image, out_image, 0); // transpose+flip(0)=CCW + } else if (config_.rotation_steps == 2) { + flip(in_image, out_image, -1); // flip(-1)=180 + } else if (config_.rotation_steps == 3) { + transpose(in_image, out_image); + flip(out_image, out_image, 1); // transpose+flip(1)=CW + } else if (config_.rotation_steps == 0) { + // Simple pass through + out_image = in_image; + } else { // if not 0,1,2,3: + RCLCPP_WARN(get_logger(), "Unknown rotation_steps %d", config_.rotation_steps); + out_image = in_image; + } + + + // Publish the image. + sensor_msgs::msg::Image::SharedPtr out_img = + cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg(); + out_img->header.frame_id = transform_.child_frame_id; + + if (cam_pub_) { + sensor_msgs::msg::CameraInfo::SharedPtr out_info(new sensor_msgs::msg::CameraInfo(*cam_info)); + out_info->header.frame_id = out_img->header.frame_id; + out_info->height = out_img->height; + out_info->width = out_img->width; + cam_pub_.publish(out_img, out_info); + } else { + img_pub_.publish(out_img); + } + + // Publish the transform. + if (tf_pub_ && (tf_unpublished_ || transform_.header.frame_id != input_frame_from_msg)) { + // Update the transform + transform_.header.frame_id = input_frame_from_msg; + transform_.child_frame_id = frameWithDefault( + config_.output_frame_id, + input_frame_from_msg + "_rotated"); + transform_.header.stamp = msg->header.stamp; + RCLCPP_WARN( + get_logger(), "Publish static transform for rotated image from %s!", + input_frame_from_msg.c_str()); + tf_pub_->sendTransform(transform_); + tf_unpublished_ = false; + } + } catch (cv::Exception & e) { + RCUTILS_LOG_ERROR( + "Image processing error: %s %s %s %i", + e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); + } + + prev_stamp_ = tf2_ros::fromMsg(msg->header.stamp); +} + +void ImageFlipNode::onInit() +{ + angle_ = 0; + prev_stamp_ = tf2::get_now(); + rclcpp::Clock::SharedPtr clock = this->get_clock(); + tf_buffer_ = std::make_shared(clock); + tf_sub_ = std::make_shared(*tf_buffer_); + + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + if (img_pub_ && img_pub_.getNumSubscribers() == 0) { + RCLCPP_DEBUG(get_logger(), "Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); + } else if (cam_pub_ && cam_pub_.getNumSubscribers() == 0) { + RCLCPP_DEBUG(get_logger(), "Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); + } else { + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + std::string topic_name = node_base->resolve_topic_or_service_name("image", false); + RCLCPP_INFO(get_logger(), "Subscribing to %s topic.", topic_name.c_str()); + + // Check that remapping appears to be correct + auto topics = this->get_topic_names_and_types(); + if (topics.find(topic_name) == topics.end()) { + RCLCPP_WARN( + get_logger(), + "Topic %s is not connected! Typical command-line usage:\n" + "\t$ ros2 run image_rotate image_flip --ros-args -r image:=", + topic_name.c_str()); + } + + // This will check image_transport parameter to get proper transport + image_transport::TransportHints transport_hint(this, "raw"); + + if (config_.use_camera_info) { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + cam_sub_ = image_transport::create_camera_subscription( + this, + topic_name, + std::bind( + &ImageFlipNode::imageCallbackWithInfo, this, + std::placeholders::_1, std::placeholders::_2), + transport_hint.getTransport(), + custom_qos); + } else { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + img_sub_ = image_transport::create_subscription( + this, + topic_name, + std::bind(&ImageFlipNode::imageCallback, this, std::placeholders::_1), + transport_hint.getTransport(), + custom_qos); + } + } + }; + + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + std::string topic = node_base->resolve_topic_or_service_name("rotated/image", false); + + auto custom_qos = rmw_qos_profile_default; + if (config_.use_camera_info) { + cam_pub_ = image_transport::create_camera_publisher(this, topic, custom_qos, pub_options); + } else { + img_pub_ = image_transport::create_publisher(this, topic, custom_qos, pub_options); + } + + tf_pub_ = std::make_shared(*this); + tf_unpublished_ = true; +} +} // namespace image_rotate + +// Register the component with class_loader. +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(image_rotate::ImageFlipNode) diff --git a/image_rotate/src/image_rotate_node.cpp b/image_rotate/src/image_rotate_node.cpp index 6b8097cc2..82832e9c7 100644 --- a/image_rotate/src/image_rotate_node.cpp +++ b/image_rotate/src/image_rotate_node.cpp @@ -278,7 +278,6 @@ void ImageRotateNode::do_work( void ImageRotateNode::onInit() { - subscriber_count_ = 0; angle_ = 0; prev_stamp_ = tf2::get_now(); rclcpp::Clock::SharedPtr clock = this->get_clock(); @@ -348,6 +347,7 @@ void ImageRotateNode::onInit() pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); img_pub_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options); } + } // namespace image_rotate #include "rclcpp_components/register_node_macro.hpp" diff --git a/image_view/CHANGELOG.rst b/image_view/CHANGELOG.rst index 8f558ee72..056a867b2 100644 --- a/image_view/CHANGELOG.rst +++ b/image_view/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package image_view ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ +* `image_view_node`: support bayer images (`#1046 `_) + so far bayer images always failed with an error: + ``` + [ERROR] [..] [image_view_node]: Unable to convert 'bayer_rggb8' image for display: 'cv_bridge.cvtColorForDisplay() does not have an output encoding that is color or mono, and has is bit in depth' + ``` + the `stereo_view_node` on the other hand already supports bayer images, + however it always forcibly converts them to monochrome, even if they are + colour images. + for now, this adds the same logic for the single-image viewer and thus + only partially resolves `#1045 `_. +* Contributors: Ralph Ursprung + 6.0.5 (2024-10-30) ------------------ diff --git a/image_view/package.xml b/image_view/package.xml index 2b47fc80d..ae7556f5e 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -2,7 +2,7 @@ image_view - 6.0.5 + 6.0.6 A simple viewer for ROS image topics. Includes a specialized viewer for stereo + disparity images. diff --git a/image_view/src/image_view_node.cpp b/image_view/src/image_view_node.cpp index 951eedd9c..c3dc1a388 100644 --- a/image_view/src/image_view_node.cpp +++ b/image_view/src/image_view_node.cpp @@ -216,6 +216,11 @@ void ImageViewNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & msg) std::string encoding = msg->encoding.empty() ? "bgr8" : msg->encoding; + // May want to view raw bayer data + if (encoding.find("bayer") != std::string::npos) { + encoding = "mono8"; + } + queued_image_.set( cv_bridge::cvtColorForDisplay( cv_bridge::toCvShare(msg), encoding, options)); diff --git a/stereo_image_proc/CHANGELOG.rst b/stereo_image_proc/CHANGELOG.rst index f8c372c4b..59df66aa9 100644 --- a/stereo_image_proc/CHANGELOG.rst +++ b/stereo_image_proc/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package stereo_image_proc ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ +* stereo_image_proc: disparity_node: Add parameter to control camera_info (`#1051 `_) + Add a parameter, use_image_transport_camera_info (default: + true), to control whether DisparityNode uses + image_transport::getCameraInfoTopic for deriving camera_info topics. + Default Behavior (backward compatible): + When use_image_transport_camera_info is true, the node continues using + image_transport::getCameraInfoTopic for camera_info resolution, + maintaining existing functionality. + Custom Behavior: + When use_image_transport_camera_info is false, the node directly uses + the camera_info topics specified via remapping (e.g., left/camera_info + and right/camera_info), bypassing image_transport's derivation logic. + This solution allows users to explicitly remap the camera_info topics + for both cameras, providing flexibility for scenarios where topic names + are not unique or need customization. + --------- + Co-authored-by: Alejandro Hernández Cordero +* Fix spelling error in topic name (`#1049 `_) +* Contributors: Michael Ferguson, quic-zhaoyuan + 6.0.5 (2024-10-30) ------------------ diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst index 8fe3ac66a..51b482bb4 100644 --- a/stereo_image_proc/doc/components.rst +++ b/stereo_image_proc/doc/components.rst @@ -89,6 +89,9 @@ Parameters over the network than camera info and/or the delay from disparity processing is too long. +*Common* + * **use_image_transport_camera_info** (bool, default: true): To control whether DisparityNode uses image_transport::getCameraInfoTopic for deriving camera_info topics. To set false, the node directly uses the camera_info topics specified via remapping (e.g., left/camera_info and right/camera_info), bypassing image_transport's derivation logic. + stereo_image_proc::PointCloudNode --------------------------------- Combines a rectified color image and disparity image to produce a @@ -106,7 +109,7 @@ Subscribed Topics image with metadata. * **left/camera_info** (sensor_msgs/CameraInfo): Left camera metadata. * **left/image_rect_color** (sensor_msgs/Image): Rectified image. - * **rigtht/camera_info** (sensor_msgs/CameraInfo): Right camera metada. + * **right/camera_info** (sensor_msgs/CameraInfo): Right camera metada. Published Topics ^^^^^^^^^^^^^^^^ diff --git a/stereo_image_proc/package.xml b/stereo_image_proc/package.xml index 85f836179..103582dcd 100644 --- a/stereo_image_proc/package.xml +++ b/stereo_image_proc/package.xml @@ -2,7 +2,7 @@ stereo_image_proc - 6.0.5 + 6.0.6 Stereo and single image rectification and disparity processing. Vincent Rabaud diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp index 8ccf60bca..da09a7d56 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -73,6 +73,7 @@ class DisparityNode : public rclcpp::Node SEMI_GLOBAL_BLOCK_MATCHING }; + bool use_image_transport_camera_info; // Subscriptions image_transport::SubscriberFilter sub_l_image_, sub_r_image_; message_filters::Subscriber sub_l_info_, sub_r_info_; @@ -170,6 +171,8 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) bool approx = this->declare_parameter("approximate_sync", false); double approx_sync_epsilon = this->declare_parameter("approximate_sync_tolerance_seconds", 0.0); this->declare_parameter("use_system_default_qos", false); + use_image_transport_camera_info = this->declare_parameter("use_image_transport_camera_info", + true); // Synchronize callbacks if (approx) { @@ -307,12 +310,20 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) std::string right_topic = node_base->resolve_topic_or_service_name("right/image_rect", false); // Allow also remapping camera_info to something different than default - std::string left_info_topic = - node_base->resolve_topic_or_service_name( - image_transport::getCameraInfoTopic(left_topic), false); - std::string right_info_topic = - node_base->resolve_topic_or_service_name( - image_transport::getCameraInfoTopic(right_topic), false); + std::string left_info_topic; + std::string right_info_topic; + + if (use_image_transport_camera_info) { + // Use image_transport to derive camera_info topics + left_info_topic = node_base->resolve_topic_or_service_name( + image_transport::getCameraInfoTopic(left_topic), false); + right_info_topic = node_base->resolve_topic_or_service_name( + image_transport::getCameraInfoTopic(right_topic), false); + } else { + // Use default camera_info topics + left_info_topic = node_base->resolve_topic_or_service_name("left/camera_info", false); + right_info_topic = node_base->resolve_topic_or_service_name("right/camera_info", false); + } // REP-2003 specifies that subscriber should be SensorDataQoS const auto sensor_data_qos = rclcpp::SensorDataQoS(); diff --git a/tracetools_image_pipeline/CHANGELOG.rst b/tracetools_image_pipeline/CHANGELOG.rst index f83f79585..8402106d1 100644 --- a/tracetools_image_pipeline/CHANGELOG.rst +++ b/tracetools_image_pipeline/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tracetools_image_pipeline ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.6 (2024-12-11) +------------------ + 6.0.5 (2024-10-30) ------------------ diff --git a/tracetools_image_pipeline/package.xml b/tracetools_image_pipeline/package.xml index c1251d11a..325832348 100644 --- a/tracetools_image_pipeline/package.xml +++ b/tracetools_image_pipeline/package.xml @@ -2,7 +2,7 @@ tracetools_image_pipeline - 6.0.5 + 6.0.6 LTTng tracing provider wrapper for image_pipeline ROS 2 meta-package.