From 5d86b030ecf0b75ad257ce1150dcf8122b2633f0 Mon Sep 17 00:00:00 2001 From: David Conner Date: Fri, 15 Apr 2022 18:56:21 -0400 Subject: [PATCH 01/16] node that flips image and publishes updated transform (e.g. for camera mounted upside down) --- image_flip/CHANGELOG.rst | 4 + image_flip/CMakeLists.txt | 32 ++ image_flip/cfg/ImageFlip.cfg | 47 +++ .../include/image_flip/image_flip_node.hpp | 104 +++++++ image_flip/include/image_flip/visibility.h | 83 ++++++ image_flip/launch/image_flip.launch.py | 45 +++ image_flip/mainpage.dox | 28 ++ image_flip/package.xml | 54 ++++ image_flip/src/image_flip.cpp | 278 ++++++++++++++++++ image_flip/src/image_flip_node.cpp | 56 ++++ 10 files changed, 731 insertions(+) create mode 100644 image_flip/CHANGELOG.rst create mode 100644 image_flip/CMakeLists.txt create mode 100755 image_flip/cfg/ImageFlip.cfg 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.cpp create mode 100644 image_flip/src/image_flip_node.cpp diff --git a/image_flip/CHANGELOG.rst b/image_flip/CHANGELOG.rst new file mode 100644 index 000000000..e75e6800b --- /dev/null +++ b/image_flip/CHANGELOG.rst @@ -0,0 +1,4 @@ +0.0.1 (2022-04-03) +------------------ +* Original release +* Based on image_rotate 2.2.1 diff --git a/image_flip/CMakeLists.txt b/image_flip/CMakeLists.txt new file mode 100644 index 000000000..20e565729 --- /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.cpp) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) +rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::ImageFlipNode") +set(node_plugins "${node_plugins}${PROJECT_NAME}::ImageFlipNode;$\n") + +ament_auto_add_executable(image_flip_node src/image_flip_node.cpp) +set_target_properties(image_flip_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME}_node) +target_link_libraries(image_flip_node ${OpenCV_LIBRARIES} ${PROJECT_NAME}) + +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/cfg/ImageFlip.cfg b/image_flip/cfg/ImageFlip.cfg new file mode 100755 index 000000000..75da28421 --- /dev/null +++ b/image_flip/cfg/ImageFlip.cfg @@ -0,0 +1,47 @@ +#! /usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2010, Willow Garage, Inc. +# All rights reserved. +# +# 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 Willow Garage, Inc. 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. + + +PACKAGE='image_flip' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +gen.add("output_frame_id", str_t, 0, "Frame to publish for the image's new orientation. Empty means add '_rotated' suffix to the image frame.", "") +gen.add("rotation_steps", double_t, 0, "number of 90 degree increments (1, 2, 3) .", 2, 1, 3) +gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to pass camera_info.", True) +gen.add("in_image_topic_name", str_t, 0, "Base input topic name ", "image") +gen.add("out_image_topic_name", str_t, 0, "Base input topic name ", "image_rotated") + + +exit(gen.generate(PACKAGE, "image_flip", "ImageFlip")) 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..d5130d75b --- /dev/null +++ b/image_flip/include/image_flip/image_flip_node.hpp @@ -0,0 +1,104 @@ +// 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; + std::string in_image_topic_name; + std::string out_image_topic_name; + +}; + +class ImageFlipNode : public rclcpp::Node +{ +public: + IMAGE_FLIP_PUBLIC ImageFlipNode(); + +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_; + + 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..37b9e6656 --- /dev/null +++ b/image_flip/launch/image_flip.launch.py @@ -0,0 +1,45 @@ +# 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. + +from launch import LaunchDescription +import launch_ros.actions + + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + package='image_rotate', node_executable='image_rotate', output='screen', + remappings=[('image', '/camera/rgb/image_raw'), + ('camera_info', '/camera/rgb/camera_info'), + ('output_image', '/camera/rotated/image_raw'), + ('output_camera_info', '/camera/rotated/camera_info']), + ]) diff --git a/image_flip/mainpage.dox b/image_flip/mainpage.dox new file mode 100644 index 000000000..7e1a0adc1 --- /dev/null +++ b/image_flip/mainpage.dox @@ -0,0 +1,28 @@ +/** +\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 ... + + + + +\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.cpp b/image_flip/src/image_flip.cpp new file mode 100644 index 000000000..765104d8e --- /dev/null +++ b/image_flip/src/image_flip.cpp @@ -0,0 +1,278 @@ +// 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() +: Node("ImageFlipNode") +{ + + 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); + config_.in_image_topic_name = this->declare_parameter("in_image_topic_name", std::string("image")); + config_.out_image_topic_name = this->declare_parameter("out_image_topic_name", std::string("rotated_image")); + + 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_)); + + } + } + + 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 not 0,1,2,3: + RCLCPP_WARN(get_logger(), "Unknown rotation_steps %d", config_.rotation_steps); + out_image = in_image; + } + + // 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; + + // 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_pub_->sendTransform(transform_); + } + + + } 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() +{ + // This is a foxy hack while waiting on rclcpp resolve_topic_name + std::string image_topic = config_.in_image_topic_name; + RCUTILS_LOG_INFO("Subscribing to image topic %s.", image_topic.c_str()); + + if (config_.use_camera_info) { + auto custom_qos = rmw_qos_profile_sensor_data; // To match Gazebo 11 pub + 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 { + auto custom_qos = rmw_qos_profile_sensor_data; // To match Gazebo 11 pub + 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 (foxy branch) + // 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(); + // This is a foxy hack while waiting on rclcpp resolve_topic_name + std::string out_image_topic = config_.out_image_topic_name; + RCUTILS_LOG_DEBUG("Advertising to image topic %s.", out_image_topic.c_str()); + auto custom_qos = rmw_qos_profile_sensor_data; // To match Gazebo 11 pub + + 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); +} +} // namespace image_flip + +#include "class_loader/register_macro.hpp" + +// Register the component with class_loader. +CLASS_LOADER_REGISTER_CLASS(image_flip::ImageFlipNode, rclcpp::Node) diff --git a/image_flip/src/image_flip_node.cpp b/image_flip/src/image_flip_node.cpp new file mode 100644 index 000000000..93d5a9c1a --- /dev/null +++ b/image_flip/src/image_flip_node.cpp @@ -0,0 +1,56 @@ +// 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. + +#include "image_flip/image_flip_node.hpp" +#include + +int main(int argc, char ** argv) +{ + // Force flush of the stdout buffer. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + if (argc <= 1) { + RCUTILS_LOG_WARN( + "Topic 'image' has not been remapped! Typical command-line usage:\n" + "\t$ ros2 run image_flip image_flip image:="); + return 1; + } + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} From b7ebae94b2fe7f1d3aa7102e1d8d455a8398809d Mon Sep 17 00:00:00 2001 From: David Conner Date: Sun, 1 May 2022 17:23:35 -0400 Subject: [PATCH 02/16] convert to ROS 2 composable nodes using rclcpp_components --- image_flip/CHANGELOG.rst | 3 + image_flip/CMakeLists.txt | 12 +- .../include/image_flip/image_flip_node.hpp | 2 +- image_flip/mainpage.dox | 4 +- image_flip/src/image_flip.cpp | 278 ------------------ image_flip/src/image_flip_node.cpp | 247 +++++++++++++++- 6 files changed, 247 insertions(+), 299 deletions(-) delete mode 100644 image_flip/src/image_flip.cpp diff --git a/image_flip/CHANGELOG.rst b/image_flip/CHANGELOG.rst index e75e6800b..bf45d5d28 100644 --- a/image_flip/CHANGELOG.rst +++ b/image_flip/CHANGELOG.rst @@ -1,3 +1,6 @@ +0.0.2 (2022-05-01) +------------------ +* Move to rclcpp_components composable node 0.0.1 (2022-04-03) ------------------ * Original release diff --git a/image_flip/CMakeLists.txt b/image_flip/CMakeLists.txt index 20e565729..69f4bf49b 100644 --- a/image_flip/CMakeLists.txt +++ b/image_flip/CMakeLists.txt @@ -15,14 +15,14 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED core imgproc) -ament_auto_add_library(${PROJECT_NAME} SHARED src/image_flip.cpp) +ament_auto_add_library(${PROJECT_NAME} SHARED src/image_flip_node.cpp) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) -rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::ImageFlipNode") -set(node_plugins "${node_plugins}${PROJECT_NAME}::ImageFlipNode;$\n") -ament_auto_add_executable(image_flip_node src/image_flip_node.cpp) -set_target_properties(image_flip_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME}_node) -target_link_libraries(image_flip_node ${OpenCV_LIBRARIES} ${PROJECT_NAME}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "${PROJECT_NAME}::ImageFlipNode" + EXECUTABLE image_flip_node +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/image_flip/include/image_flip/image_flip_node.hpp b/image_flip/include/image_flip/image_flip_node.hpp index d5130d75b..78797101d 100644 --- a/image_flip/include/image_flip/image_flip_node.hpp +++ b/image_flip/include/image_flip/image_flip_node.hpp @@ -62,7 +62,7 @@ struct ImageFlipConfig class ImageFlipNode : public rclcpp::Node { public: - IMAGE_FLIP_PUBLIC ImageFlipNode(); + IMAGE_FLIP_PUBLIC ImageFlipNode(rclcpp::NodeOptions options); private: const std::string frameWithDefault(const std::string & frame, const std::string & image_frame); diff --git a/image_flip/mainpage.dox b/image_flip/mainpage.dox index 7e1a0adc1..feed2e6e9 100644 --- a/image_flip/mainpage.dox +++ b/image_flip/mainpage.dox @@ -4,7 +4,9 @@ \b image_flip is a simpler image_rotate limited to 90 degree rotations. -It preserves the size data and publishes a camera_info topic ... +It preserves the size data and publishes a camera_info topic, +as well as a tf transform between original optical frame and rotated frame +...