From 0dd99bf0f8afd84e849c5dfc69b2c7efb37df890 Mon Sep 17 00:00:00 2001 From: Stuart Alldritt Date: Mon, 25 Nov 2024 01:32:41 -0800 Subject: [PATCH 1/7] 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_. --- depth_image_proc/src/register.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index 7db28843..690d28f0 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 From a7283c73d8e16101f4fe7908222c16e9c55a6f14 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 27 Nov 2024 15:22:12 -0500 Subject: [PATCH 2/7] Fix spelling error in topic name (#1049) --- stereo_image_proc/doc/components.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst index 8fe3ac66..1fab34c9 100644 --- a/stereo_image_proc/doc/components.rst +++ b/stereo_image_proc/doc/components.rst @@ -106,7 +106,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 ^^^^^^^^^^^^^^^^ From bb4ab63f19ec4d7df9cd80a0785ed2e4ffa144d1 Mon Sep 17 00:00:00 2001 From: quic-zhaoyuan <164289792+quic-zhaoyuan@users.noreply.github.com> Date: Fri, 29 Nov 2024 04:02:45 +0800 Subject: [PATCH 3/7] stereo_image_proc: disparity_node: Add parameter to control camera_info (#1051) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --------- Signed-off-by: Zhaoyuan Cheng Signed-off-by: Zhaoyuan Co-authored-by: Alejandro Hernández Cordero --- stereo_image_proc/doc/components.rst | 3 +++ .../src/stereo_image_proc/disparity_node.cpp | 23 ++++++++++++++----- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst index 1fab34c9..51b482bb 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 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 8ccf60bc..da09a7d5 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(); From 6ff605c129e85f8fb9cfaef5e146c9a76ef1cbf1 Mon Sep 17 00:00:00 2001 From: Ralph Ursprung <39383228+rursprung@users.noreply.github.com> Date: Tue, 10 Dec 2024 23:27:41 +0100 Subject: [PATCH 4/7] `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. --- image_view/src/image_view_node.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/image_view/src/image_view_node.cpp b/image_view/src/image_view_node.cpp index 951eedd9..c3dc1a38 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)); From c8622e7131cf8a1e27350e4d4bc99abab51f69f0 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 11 Dec 2024 04:55:20 -0500 Subject: [PATCH 5/7] add image_flip node (#942) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- image_rotate/CMakeLists.txt | 7 + image_rotate/doc/components.rst | 30 ++ .../include/image_rotate/image_flip.hpp | 103 +++++++ .../image_rotate/image_rotate_node.hpp | 3 +- image_rotate/launch/image_flip.launch.py | 49 ++++ image_rotate/package.xml | 1 + image_rotate/src/image_flip.cpp | 261 ++++++++++++++++++ image_rotate/src/image_rotate_node.cpp | 2 +- 8 files changed, 454 insertions(+), 2 deletions(-) create mode 100644 image_rotate/include/image_rotate/image_flip.hpp create mode 100644 image_rotate/launch/image_flip.launch.py create mode 100644 image_rotate/src/image_flip.cpp diff --git a/image_rotate/CMakeLists.txt b/image_rotate/CMakeLists.txt index 49c3b94f..4e172090 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 cb6b7727..daa9c191 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 00000000..7285567d --- /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 67e08eae..b65c189a 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 00000000..be093053 --- /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 d0e84b25..5b9b601b 100644 --- a/image_rotate/package.xml +++ b/image_rotate/package.xml @@ -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 00000000..3070db65 --- /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 6b8097cc..82832e9c 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" From de9d3927e0b373670852564343b7f128aa6b81ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 11 Dec 2024 16:43:18 +0100 Subject: [PATCH 6/7] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- camera_calibration/CHANGELOG.rst | 3 +++ depth_image_proc/CHANGELOG.rst | 7 +++++++ image_pipeline/CHANGELOG.rst | 3 +++ image_proc/CHANGELOG.rst | 3 +++ image_publisher/CHANGELOG.rst | 3 +++ image_rotate/CHANGELOG.rst | 16 ++++++++++++++++ image_view/CHANGELOG.rst | 14 ++++++++++++++ stereo_image_proc/CHANGELOG.rst | 22 ++++++++++++++++++++++ tracetools_image_pipeline/CHANGELOG.rst | 3 +++ 9 files changed, 74 insertions(+) diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst index f6054d6f..60bd4b8b 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/depth_image_proc/CHANGELOG.rst b/depth_image_proc/CHANGELOG.rst index 1716878c..fdf48fa9 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/image_pipeline/CHANGELOG.rst b/image_pipeline/CHANGELOG.rst index 3b56167f..93b086ab 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_proc/CHANGELOG.rst b/image_proc/CHANGELOG.rst index e2dc58ad..d6243d51 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_publisher/CHANGELOG.rst b/image_publisher/CHANGELOG.rst index daad6c57..a41f6873 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_rotate/CHANGELOG.rst b/image_rotate/CHANGELOG.rst index e4510758..22739e5b 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_view/CHANGELOG.rst b/image_view/CHANGELOG.rst index 8f558ee7..056a867b 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/stereo_image_proc/CHANGELOG.rst b/stereo_image_proc/CHANGELOG.rst index f8c372c4..59df66aa 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/tracetools_image_pipeline/CHANGELOG.rst b/tracetools_image_pipeline/CHANGELOG.rst index f83f7958..8402106d 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) ------------------ From afc34b477f18492901d08174a3f0036f50952132 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 11 Dec 2024 16:43:34 +0100 Subject: [PATCH 7/7] 6.0.6 --- camera_calibration/package.xml | 2 +- camera_calibration/setup.py | 2 +- depth_image_proc/package.xml | 2 +- image_pipeline/package.xml | 2 +- image_proc/package.xml | 2 +- image_publisher/package.xml | 2 +- image_rotate/package.xml | 2 +- image_view/package.xml | 2 +- stereo_image_proc/package.xml | 2 +- tracetools_image_pipeline/package.xml | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml index 330cda52..afb306b3 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 401ea9fa..e8771623 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/package.xml b/depth_image_proc/package.xml index 3092553a..428ae653 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/image_pipeline/package.xml b/image_pipeline/package.xml index 2ec7beb1..57327355 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/package.xml b/image_proc/package.xml index 70debc4f..3720c969 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/package.xml b/image_publisher/package.xml index 38c5dc53..a621501f 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/package.xml b/image_rotate/package.xml index 5b9b601b..1cd60aa2 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 diff --git a/image_view/package.xml b/image_view/package.xml index 2b47fc80..ae7556f5 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/stereo_image_proc/package.xml b/stereo_image_proc/package.xml index 85f83617..103582dc 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/tracetools_image_pipeline/package.xml b/tracetools_image_pipeline/package.xml index c1251d11..32583234 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.