From bca58a4666521542c13c0ac28d5db1d3bf47030d Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 15 Apr 2022 18:56:21 -0400 Subject: [PATCH 1/3] node that flips image and publishes updated transform (e.g. for camera mounted upside down) --- image_flip/CMakeLists.txt | 32 ++ .../include/image_flip/image_flip_node.hpp | 106 +++++++ image_flip/include/image_flip/visibility.h | 83 +++++ image_flip/launch/image_flip.launch.py | 64 ++++ image_flip/mainpage.dox | 30 ++ image_flip/package.xml | 54 ++++ image_flip/src/image_flip_node.cpp | 295 ++++++++++++++++++ 7 files changed, 664 insertions(+) create mode 100644 image_flip/CMakeLists.txt create mode 100644 image_flip/include/image_flip/image_flip_node.hpp create mode 100644 image_flip/include/image_flip/visibility.h create mode 100644 image_flip/launch/image_flip.launch.py create mode 100644 image_flip/mainpage.dox create mode 100644 image_flip/package.xml create mode 100644 image_flip/src/image_flip_node.cpp diff --git a/image_flip/CMakeLists.txt b/image_flip/CMakeLists.txt new file mode 100644 index 000000000..69f4bf49b --- /dev/null +++ b/image_flip/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(image_flip) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED core imgproc) + +ament_auto_add_library(${PROJECT_NAME} SHARED src/image_flip_node.cpp) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "${PROJECT_NAME}::ImageFlipNode" + EXECUTABLE image_flip_node +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/image_flip/include/image_flip/image_flip_node.hpp b/image_flip/include/image_flip/image_flip_node.hpp new file mode 100644 index 000000000..b3bba81ae --- /dev/null +++ b/image_flip/include/image_flip/image_flip_node.hpp @@ -0,0 +1,106 @@ +// 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_FLIP__IMAGE_FLIP_NODE_HPP_ +#define IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "image_flip/visibility.h" + +namespace image_flip +{ + +struct ImageFlipConfig +{ + std::string output_frame_id; + int rotation_steps; + bool use_camera_info; + rmw_qos_profile_t input_qos; // "default", "sensor_data", etc .. + rmw_qos_profile_t output_qos; +}; + +class ImageFlipNode : public rclcpp::Node +{ +public: + IMAGE_FLIP_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 subscribe(); + void unsubscribe(); + void connectCb(); + void disconnectCb(); + 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_; + + image_flip::ImageFlipConfig config_; + + image_transport::Publisher img_pub_; + image_transport::Subscriber img_sub_; + image_transport::CameraSubscriber cam_sub_; + image_transport::CameraPublisher cam_pub_; + + int subscriber_count_; + double angle_; + tf2::TimePoint prev_stamp_; + geometry_msgs::msg::TransformStamped transform_; +}; +} // namespace image_flip + +#endif // IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_ diff --git a/image_flip/include/image_flip/visibility.h b/image_flip/include/image_flip/visibility.h new file mode 100644 index 000000000..c18aa73e3 --- /dev/null +++ b/image_flip/include/image_flip/visibility.h @@ -0,0 +1,83 @@ +// 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_FLIP__VISIBILITY_H_ +#define IMAGE_FLIP__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + + #ifdef __GNUC__ + #define IMAGE_FLIP_EXPORT __attribute__ ((dllexport)) + #define IMAGE_FLIP_IMPORT __attribute__ ((dllimport)) + #else + #define IMAGE_FLIP_EXPORT __declspec(dllexport) + #define IMAGE_FLIP_IMPORT __declspec(dllimport) + #endif + + #ifdef IMAGE_FLIP_DLL + #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_EXPORT + #else + #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_IMPORT + #endif + + #define IMAGE_FLIP_PUBLIC_TYPE IMAGE_FLIP_PUBLIC + + #define IMAGE_FLIP_LOCAL + +#else + + #define IMAGE_FLIP_EXPORT __attribute__ ((visibility("default"))) + #define IMAGE_FLIP_IMPORT + + #if __GNUC__ >= 4 + #define IMAGE_FLIP_PUBLIC __attribute__ ((visibility("default"))) + #define IMAGE_FLIP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define IMAGE_FLIP_PUBLIC + #define IMAGE_FLIP_LOCAL + #endif + + #define IMAGE_FLIP_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // IMAGE_FLIP__VISIBILITY_H_ diff --git a/image_flip/launch/image_flip.launch.py b/image_flip/launch/image_flip.launch.py new file mode 100644 index 000000000..500b5cb2f --- /dev/null +++ b/image_flip/launch/image_flip.launch.py @@ -0,0 +1,64 @@ +# 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.""" + +import launch_ros.actions +from launch import LaunchDescription + + +def generate_launch_description(): + """ + Launch description for basic launch of the image_flip_node. + + Launch description for basic launch of the image_flip_node, + which includes parameters and topic remappings. + """ + return LaunchDescription([ + launch_ros.actions.Node( + package='image_flip', executable='image_flip_node', + output='screen', name='camera_flip', + remappings=[("image", 'camera/rgb/image_raw'), + ('camera_info', 'camera/rgb/camera_info'), + ('rotated/image', 'camera_rotated/image_rotated'), + ('rotated/camera_info', 'camera_rotated/camera_info'), + ('rotated/image/compressed', + 'camera_rotated/image_rotated/compressed'), + ('rotated/image/compressedDepth', + 'camera_rotated/image_rotated/compressedDepth'), + ('rotated/image/theora', + 'camera_rotated/image_rotated/theora')], + parameters=[{'output_frame_id': "camera_rotated", + 'rotation_steps': 2, + 'use_camera_info': True, + 'input_qos': 'best_effort', + 'output_qos': 'default'}])]) diff --git a/image_flip/mainpage.dox b/image_flip/mainpage.dox new file mode 100644 index 000000000..feed2e6e9 --- /dev/null +++ b/image_flip/mainpage.dox @@ -0,0 +1,30 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b image_flip is a simpler image_rotate limited to 90 degree rotations. + +It preserves the size data and publishes a camera_info topic, +as well as a tf transform between original optical frame and rotated frame +... + + + + +\section codeapi Code API + + + + +*/ diff --git a/image_flip/package.xml b/image_flip/package.xml new file mode 100644 index 000000000..77d615b0d --- /dev/null +++ b/image_flip/package.xml @@ -0,0 +1,54 @@ + + + + image_flip + 2.2.1 + +

+ Contains a node that rotates an image stream in 90 degree increments + about the optical axis. + + This is simpler and faster than the general image_rotate +

+

+ This node is intended to allow camera images to be visualized in an + orientation that is more intuitive than the hardware-constrained + orientation of the physical camera, but is limited to fixed mountings + in 90 degree increments. + +

+

+ This node outputs a camera_info, but users are cautioned that the + distortion data is not corrected for flipping and is only valid if + symmetric. + +

+ + + David Conner + + BSD + https://index.ros.org/p/image_pipeline/ + https://github.com/ros-perception/image_pipeline/issues + https://github.com/ros-perception/image_pipeline + David Conner + + ament_cmake_auto + + class_loader + + cv_bridge + image_transport + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/image_flip/src/image_flip_node.cpp b/image_flip/src/image_flip_node.cpp new file mode 100644 index 000000000..92396ac0a --- /dev/null +++ b/image_flip/src/image_flip_node.cpp @@ -0,0 +1,295 @@ +// 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. + +/******************************************************************** +* image_flip_node.cpp +* this is a forked version of image_flip. +* this image_flip_node supports: +* 1) node +* 2) tf and tf2 +*********************************************************************/ + +#include "image_flip/image_flip_node.hpp" + +#include +#include +#include +#include + +namespace image_flip +{ + +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); + + // Assume best_effort (sensor_data) on subscriber + // which is compatible with reliable publisher + // Assume default reliable on publisher so it will be compatible with both + // reliable and best_effort subscriptions + std::string qos_type = this->declare_parameter("input_qos", "sensor_data"); + if (qos_type == "sensor_data") { + config_.input_qos = rmw_qos_profile_sensor_data; + } else { + config_.input_qos = rmw_qos_profile_default; + } + + qos_type = this->declare_parameter("output_qos", "default"); + if (qos_type == "sensor_data") { + config_.output_qos = rmw_qos_profile_sensor_data; + } else { + config_.output_qos = rmw_qos_profile_default; + } + + auto reconfigureCallback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + RCLCPP_INFO(get_logger(), "reconfigureCallback"); + + 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; + } + } + + if (subscriber_count_) { // @todo: Could do this without an interruption at some point. + unsubscribe(); + subscribe(); + } + 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_id = cam_info->header.frame_id; + if (frame_id.length() == 0) { + frame_id = msg->header.frame_id; + } + do_work(msg, cam_info, frame_id); +} + +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::subscribe() +{ + std::string image_topic = "image"; + auto custom_qos = config_.input_qos; + + RCUTILS_LOG_INFO("Subscribing to image topic %s.", image_topic.c_str()); + + if (config_.use_camera_info) { + cam_sub_ = image_transport::create_camera_subscription( + this, + image_topic, // "image", + std::bind( + &ImageFlipNode::imageCallbackWithInfo, this, + std::placeholders::_1, std::placeholders::_2), + "raw", + custom_qos); + } else { + img_sub_ = image_transport::create_subscription( + this, + image_topic, // "image", + std::bind(&ImageFlipNode::imageCallback, this, std::placeholders::_1), + "raw", + custom_qos); + } +} + +void ImageFlipNode::unsubscribe() +{ + RCUTILS_LOG_DEBUG("Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); +} + +void ImageFlipNode::connectCb() +{ + if (subscriber_count_++ == 0) { + subscribe(); + } +} + +void ImageFlipNode::disconnectCb() +{ + subscriber_count_--; + if (subscriber_count_ == 0) { + unsubscribe(); + } +} + +void ImageFlipNode::onInit() +{ + subscriber_count_ = 0; + 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_); + + // --- Note: From image_rotate (rolling branch 19-Dec-22)) + // TODO(yechun1): Implement when SubscriberStatusCallback is available + // image_transport::SubscriberStatusCallback connect_cb = + // boost::bind(&ImageFlipNode::connectCb, this, _1); + // image_transport::SubscriberStatusCallback connect_cb = + // std::bind(&CropForemostNode::connectCb, this); + // image_transport::SubscriberStatusCallback disconnect_cb = + // boost::bind(&ImageFlipNode::disconnectCb, this, _1); + // img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise( + // "image", 1, connect_cb, disconnect_cb); + //---------------------------------------------------- + + connectCb(); + + std::string out_image_topic = "rotated/image"; + RCUTILS_LOG_DEBUG("Advertising to image topic %s.", out_image_topic.c_str()); + auto custom_qos = config_.output_qos; + + if (config_.use_camera_info) { + cam_pub_ = image_transport::create_camera_publisher(this, out_image_topic, custom_qos); + } else { + img_pub_ = image_transport::create_publisher(this, out_image_topic, custom_qos); + } + + tf_pub_ = std::make_shared(*this); + tf_unpublished_ = true; +} +} // namespace image_flip + +// Register the component with class_loader. +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(image_flip::ImageFlipNode) From c5787d9435811edcad05a4d3da87ae1a59485fc1 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 12 Feb 2024 13:28:10 -0500 Subject: [PATCH 2/3] migrate to image_rotate, make lazy --- image_flip/CMakeLists.txt | 32 ---- image_flip/include/image_flip/visibility.h | 83 -------- image_flip/mainpage.dox | 30 --- image_flip/package.xml | 54 ------ image_rotate/CMakeLists.txt | 7 + .../include/image_rotate/image_flip.hpp | 29 ++- .../image_rotate/image_rotate_node.hpp | 3 +- .../launch/image_flip.launch.py | 25 +-- image_rotate/package.xml | 1 + .../src/image_flip.cpp | 178 +++++++----------- image_rotate/src/image_rotate_node.cpp | 2 +- 11 files changed, 102 insertions(+), 342 deletions(-) delete mode 100644 image_flip/CMakeLists.txt delete mode 100644 image_flip/include/image_flip/visibility.h delete mode 100644 image_flip/mainpage.dox delete mode 100644 image_flip/package.xml rename image_flip/include/image_flip/image_flip_node.hpp => image_rotate/include/image_rotate/image_flip.hpp (86%) rename {image_flip => image_rotate}/launch/image_flip.launch.py (70%) rename image_flip/src/image_flip_node.cpp => image_rotate/src/image_flip.cpp (66%) diff --git a/image_flip/CMakeLists.txt b/image_flip/CMakeLists.txt deleted file mode 100644 index 69f4bf49b..000000000 --- a/image_flip/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(image_flip) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -find_package(OpenCV REQUIRED core imgproc) - -ament_auto_add_library(${PROJECT_NAME} SHARED src/image_flip_node.cpp) -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "${PROJECT_NAME}::ImageFlipNode" - EXECUTABLE image_flip_node -) - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/image_flip/include/image_flip/visibility.h b/image_flip/include/image_flip/visibility.h deleted file mode 100644 index c18aa73e3..000000000 --- a/image_flip/include/image_flip/visibility.h +++ /dev/null @@ -1,83 +0,0 @@ -// 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_FLIP__VISIBILITY_H_ -#define IMAGE_FLIP__VISIBILITY_H_ - -#ifdef __cplusplus -extern "C" -{ -#endif - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - - #ifdef __GNUC__ - #define IMAGE_FLIP_EXPORT __attribute__ ((dllexport)) - #define IMAGE_FLIP_IMPORT __attribute__ ((dllimport)) - #else - #define IMAGE_FLIP_EXPORT __declspec(dllexport) - #define IMAGE_FLIP_IMPORT __declspec(dllimport) - #endif - - #ifdef IMAGE_FLIP_DLL - #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_EXPORT - #else - #define IMAGE_FLIP_PUBLIC IMAGE_FLIP_IMPORT - #endif - - #define IMAGE_FLIP_PUBLIC_TYPE IMAGE_FLIP_PUBLIC - - #define IMAGE_FLIP_LOCAL - -#else - - #define IMAGE_FLIP_EXPORT __attribute__ ((visibility("default"))) - #define IMAGE_FLIP_IMPORT - - #if __GNUC__ >= 4 - #define IMAGE_FLIP_PUBLIC __attribute__ ((visibility("default"))) - #define IMAGE_FLIP_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define IMAGE_FLIP_PUBLIC - #define IMAGE_FLIP_LOCAL - #endif - - #define IMAGE_FLIP_PUBLIC_TYPE -#endif - -#ifdef __cplusplus -} -#endif - -#endif // IMAGE_FLIP__VISIBILITY_H_ diff --git a/image_flip/mainpage.dox b/image_flip/mainpage.dox deleted file mode 100644 index feed2e6e9..000000000 --- a/image_flip/mainpage.dox +++ /dev/null @@ -1,30 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b image_flip is a simpler image_rotate limited to 90 degree rotations. - -It preserves the size data and publishes a camera_info topic, -as well as a tf transform between original optical frame and rotated frame -... - - - - -\section codeapi Code API - - - - -*/ diff --git a/image_flip/package.xml b/image_flip/package.xml deleted file mode 100644 index 77d615b0d..000000000 --- a/image_flip/package.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - image_flip - 2.2.1 - -

- Contains a node that rotates an image stream in 90 degree increments - about the optical axis. - - This is simpler and faster than the general image_rotate -

-

- This node is intended to allow camera images to be visualized in an - orientation that is more intuitive than the hardware-constrained - orientation of the physical camera, but is limited to fixed mountings - in 90 degree increments. - -

-

- This node outputs a camera_info, but users are cautioned that the - distortion data is not corrected for flipping and is only valid if - symmetric. - -

- - - David Conner - - BSD - https://index.ros.org/p/image_pipeline/ - https://github.com/ros-perception/image_pipeline/issues - https://github.com/ros-perception/image_pipeline - David Conner - - ament_cmake_auto - - class_loader - - cv_bridge - image_transport - rclcpp - rclcpp_components - tf2 - tf2_geometry_msgs - tf2_ros - - ament_lint_auto - ament_lint_common - - - ament_cmake - - 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_flip/include/image_flip/image_flip_node.hpp b/image_rotate/include/image_rotate/image_flip.hpp similarity index 86% rename from image_flip/include/image_flip/image_flip_node.hpp rename to image_rotate/include/image_rotate/image_flip.hpp index b3bba81ae..7285567d2 100644 --- a/image_flip/include/image_flip/image_flip_node.hpp +++ b/image_rotate/include/image_rotate/image_flip.hpp @@ -30,8 +30,9 @@ // 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_FLIP__IMAGE_FLIP_NODE_HPP_ -#define IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_ + +#ifndef IMAGE_ROTATE__IMAGE_FLIP_HPP_ +#define IMAGE_ROTATE__IMAGE_FLIP_HPP_ #include @@ -47,9 +48,9 @@ #include #include -#include "image_flip/visibility.h" +#include "image_rotate/visibility.h" -namespace image_flip +namespace image_rotate { struct ImageFlipConfig @@ -57,14 +58,12 @@ struct ImageFlipConfig std::string output_frame_id; int rotation_steps; bool use_camera_info; - rmw_qos_profile_t input_qos; // "default", "sensor_data", etc .. - rmw_qos_profile_t output_qos; }; class ImageFlipNode : public rclcpp::Node { public: - IMAGE_FLIP_PUBLIC ImageFlipNode(rclcpp::NodeOptions options); + IMAGE_ROTATE_PUBLIC ImageFlipNode(rclcpp::NodeOptions options); private: const std::string frameWithDefault(const std::string & frame, const std::string & image_frame); @@ -76,10 +75,6 @@ class ImageFlipNode : public rclcpp::Node const sensor_msgs::msg::Image::ConstSharedPtr & msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info, const std::string input_frame_from_msg); - void subscribe(); - void unsubscribe(); - void connectCb(); - void disconnectCb(); void onInit(); rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; @@ -89,18 +84,20 @@ class ImageFlipNode : public rclcpp::Node std::shared_ptr tf_pub_; bool tf_unpublished_; - image_flip::ImageFlipConfig config_; + ImageFlipConfig 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_; + + // Publisher - only one is used at a time - depends on use_camera_info + image_transport::Publisher img_pub_; image_transport::CameraPublisher cam_pub_; - int subscriber_count_; double angle_; tf2::TimePoint prev_stamp_; geometry_msgs::msg::TransformStamped transform_; }; -} // namespace image_flip +} // namespace image_rotate -#endif // IMAGE_FLIP__IMAGE_FLIP_NODE_HPP_ +#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_flip/launch/image_flip.launch.py b/image_rotate/launch/image_flip.launch.py similarity index 70% rename from image_flip/launch/image_flip.launch.py rename to image_rotate/launch/image_flip.launch.py index 500b5cb2f..191ee2907 100644 --- a/image_flip/launch/image_flip.launch.py +++ b/image_rotate/launch/image_flip.launch.py @@ -32,32 +32,19 @@ """Demonstration of basic launch of the image_flip_node with remappings.""" -import launch_ros.actions from launch import LaunchDescription +import launch_ros.actions def generate_launch_description(): - """ - Launch description for basic launch of the image_flip_node. - - Launch description for basic launch of the image_flip_node, - which includes parameters and topic remappings. - """ + """Launch description for basic launch of the image_flip.""" return LaunchDescription([ launch_ros.actions.Node( - package='image_flip', executable='image_flip_node', + package='image_rotate', executable='image_flip', output='screen', name='camera_flip', - remappings=[("image", 'camera/rgb/image_raw'), - ('camera_info', 'camera/rgb/camera_info'), - ('rotated/image', 'camera_rotated/image_rotated'), - ('rotated/camera_info', 'camera_rotated/camera_info'), - ('rotated/image/compressed', - 'camera_rotated/image_rotated/compressed'), - ('rotated/image/compressedDepth', - 'camera_rotated/image_rotated/compressedDepth'), - ('rotated/image/theora', - 'camera_rotated/image_rotated/theora')], - parameters=[{'output_frame_id': "camera_rotated", + 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, 'input_qos': 'best_effort', diff --git a/image_rotate/package.xml b/image_rotate/package.xml index d0e84b250..5b9b601b7 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_flip/src/image_flip_node.cpp b/image_rotate/src/image_flip.cpp similarity index 66% rename from image_flip/src/image_flip_node.cpp rename to image_rotate/src/image_flip.cpp index 92396ac0a..3070db651 100644 --- a/image_flip/src/image_flip_node.cpp +++ b/image_rotate/src/image_flip.cpp @@ -32,22 +32,14 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -/******************************************************************** -* image_flip_node.cpp -* this is a forked version of image_flip. -* this image_flip_node supports: -* 1) node -* 2) tf and tf2 -*********************************************************************/ - -#include "image_flip/image_flip_node.hpp" +#include "image_rotate/image_flip.hpp" #include #include #include #include -namespace image_flip +namespace image_rotate { ImageFlipNode::ImageFlipNode(rclcpp::NodeOptions options) @@ -57,29 +49,12 @@ ImageFlipNode::ImageFlipNode(rclcpp::NodeOptions options) config_.rotation_steps = this->declare_parameter("rotation_steps", 2); config_.use_camera_info = this->declare_parameter("use_camera_info", true); - // Assume best_effort (sensor_data) on subscriber - // which is compatible with reliable publisher - // Assume default reliable on publisher so it will be compatible with both - // reliable and best_effort subscriptions - std::string qos_type = this->declare_parameter("input_qos", "sensor_data"); - if (qos_type == "sensor_data") { - config_.input_qos = rmw_qos_profile_sensor_data; - } else { - config_.input_qos = rmw_qos_profile_default; - } - - qos_type = this->declare_parameter("output_qos", "default"); - if (qos_type == "sensor_data") { - config_.output_qos = rmw_qos_profile_sensor_data; - } else { - config_.output_qos = rmw_qos_profile_default; - } + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); auto reconfigureCallback = [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult { - RCLCPP_INFO(get_logger(), "reconfigureCallback"); - auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { @@ -95,11 +70,6 @@ ImageFlipNode::ImageFlipNode(rclcpp::NodeOptions options) tf_unpublished_ = true; } } - - if (subscriber_count_) { // @todo: Could do this without an interruption at some point. - unsubscribe(); - subscribe(); - } return result; }; on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(reconfigureCallback); @@ -125,11 +95,11 @@ void ImageFlipNode::imageCallbackWithInfo( const sensor_msgs::msg::Image::ConstSharedPtr & msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) { - std::string frame_id = cam_info->header.frame_id; - if (frame_id.length() == 0) { - frame_id = msg->header.frame_id; + std::string frame = cam_info->header.frame_id; + if (frame.empty()) { + frame = msg->header.frame_id; } - do_work(msg, cam_info, frame_id); + do_work(msg, cam_info, frame); } void ImageFlipNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) @@ -204,92 +174,88 @@ void ImageFlipNode::do_work( prev_stamp_ = tf2_ros::fromMsg(msg->header.stamp); } -void ImageFlipNode::subscribe() -{ - std::string image_topic = "image"; - auto custom_qos = config_.input_qos; - - RCUTILS_LOG_INFO("Subscribing to image topic %s.", image_topic.c_str()); - - if (config_.use_camera_info) { - cam_sub_ = image_transport::create_camera_subscription( - this, - image_topic, // "image", - std::bind( - &ImageFlipNode::imageCallbackWithInfo, this, - std::placeholders::_1, std::placeholders::_2), - "raw", - custom_qos); - } else { - img_sub_ = image_transport::create_subscription( - this, - image_topic, // "image", - std::bind(&ImageFlipNode::imageCallback, this, std::placeholders::_1), - "raw", - custom_qos); - } -} - -void ImageFlipNode::unsubscribe() -{ - RCUTILS_LOG_DEBUG("Unsubscribing from image topic."); - img_sub_.shutdown(); - cam_sub_.shutdown(); -} - -void ImageFlipNode::connectCb() -{ - if (subscriber_count_++ == 0) { - subscribe(); - } -} - -void ImageFlipNode::disconnectCb() -{ - subscriber_count_--; - if (subscriber_count_ == 0) { - unsubscribe(); - } -} - void ImageFlipNode::onInit() { - subscriber_count_ = 0; 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_); - // --- Note: From image_rotate (rolling branch 19-Dec-22)) - // TODO(yechun1): Implement when SubscriberStatusCallback is available - // image_transport::SubscriberStatusCallback connect_cb = - // boost::bind(&ImageFlipNode::connectCb, this, _1); - // image_transport::SubscriberStatusCallback connect_cb = - // std::bind(&CropForemostNode::connectCb, this); - // image_transport::SubscriberStatusCallback disconnect_cb = - // boost::bind(&ImageFlipNode::disconnectCb, this, _1); - // img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise( - // "image", 1, connect_cb, disconnect_cb); - //---------------------------------------------------- + // 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()); + } - connectCb(); + // 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); + } + } + }; - std::string out_image_topic = "rotated/image"; - RCUTILS_LOG_DEBUG("Advertising to image topic %s.", out_image_topic.c_str()); - auto custom_qos = config_.output_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, out_image_topic, custom_qos); + cam_pub_ = image_transport::create_camera_publisher(this, topic, custom_qos, pub_options); } else { - img_pub_ = image_transport::create_publisher(this, out_image_topic, custom_qos); + img_pub_ = image_transport::create_publisher(this, topic, custom_qos, pub_options); } tf_pub_ = std::make_shared(*this); tf_unpublished_ = true; } -} // namespace image_flip +} // namespace image_rotate // Register the component with class_loader. #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(image_flip::ImageFlipNode) +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" From 1aa182dd7b096628ee9372bb24af62cfef61258a Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 6 Dec 2024 11:14:44 -0500 Subject: [PATCH 3/3] add docs --- image_rotate/doc/components.rst | 30 ++++++++++++++++++++++++ image_rotate/launch/image_flip.launch.py | 4 +--- 2 files changed, 31 insertions(+), 3 deletions(-) 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/launch/image_flip.launch.py b/image_rotate/launch/image_flip.launch.py index 191ee2907..be0930535 100644 --- a/image_rotate/launch/image_flip.launch.py +++ b/image_rotate/launch/image_flip.launch.py @@ -46,6 +46,4 @@ def generate_launch_description(): ('rotated/image', 'camera_rotated/image_rotated')], parameters=[{'output_frame_id': 'camera_rotated', 'rotation_steps': 2, - 'use_camera_info': True, - 'input_qos': 'best_effort', - 'output_qos': 'default'}])]) + 'use_camera_info': True}])])