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..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_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"