diff --git a/image_rotate/CMakeLists.txt b/image_rotate/CMakeLists.txt index 147b1db3d..7d2269541 100644 --- a/image_rotate/CMakeLists.txt +++ b/image_rotate/CMakeLists.txt @@ -1,32 +1,61 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(image_rotate) -find_package(catkin REQUIRED COMPONENTS cmake_modules cv_bridge dynamic_reconfigure image_transport nodelet roscpp tf2 tf2_geometry_msgs) - -# generate the dynamic_reconfigure config file -generate_dynamic_reconfigure_options(cfg/ImageRotate.cfg) - -catkin_package() - +# 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 REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(class_loader REQUIRED) +find_package(image_transport REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(OpenCV REQUIRED core imgproc) # add the executable -include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) +include_directories(include) + +add_library(${PROJECT_NAME} SHARED src/image_rotate_node.cpp) +target_link_libraries(${PROJECT_NAME} ${ament_LIBRARIES} ${OpenCV_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} + "image_transport" + "cv_bridge" + "class_loader" + "rclcpp" + "tf2_ros" + "tf2_geometry_msgs" +) +rclcpp_register_node_plugins(${PROJECT_NAME} "${PROJECT_NAME}::ImageRotateNode") +set(node_plugins "${node_plugins}${PROJECT_NAME}::ImageRotateNode;$\n") -add_library(${PROJECT_NAME} SHARED src/nodelet/image_rotate_nodelet.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) -install(TARGETS image_rotate - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install(TARGETS ${PROJECT_NAME} + DESTINATION lib ) -add_executable(image_rotate_exe src/node/image_rotate.cpp) -SET_TARGET_PROPERTIES(image_rotate_exe PROPERTIES OUTPUT_NAME image_rotate) -target_link_libraries(image_rotate_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +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 ${ament_LIBRARIES} ${OpenCV_LIBRARIES} ${PROJECT_NAME}) +ament_target_dependencies(image_rotate_bin + "rclcpp") -install(TARGETS image_rotate_exe - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS image_rotate_bin + DESTINATION lib/${PROJECT_NAME} ) -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY examples/launch + DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/image_rotate/COLCON_IGNORE b/image_rotate/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/image_rotate/examples/launch/image_rotate.launch.py b/image_rotate/examples/launch/image_rotate.launch.py new file mode 100644 index 000000000..7345a5ea8 --- /dev/null +++ b/image_rotate/examples/launch/image_rotate.launch.py @@ -0,0 +1,44 @@ +# 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. + +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/color/image_raw'), + ('camera_info', '/camera/color/camera_info'), + ('rotated/image', '/camera/color/image_raw_rotated')]), + ]) diff --git a/image_rotate/include/image_rotate/image_rotate_node.hpp b/image_rotate/include/image_rotate/image_rotate_node.hpp new file mode 100644 index 000000000..538cf7629 --- /dev/null +++ b/image_rotate/include/image_rotate/image_rotate_node.hpp @@ -0,0 +1,108 @@ +// 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_ROTATE_NODE_HPP_ +#define IMAGE_ROTATE__IMAGE_ROTATE_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "image_rotate/visibility.h" + +namespace image_rotate +{ + +struct ImageRotateConfig +{ + std::string target_frame_id; + double target_x; + double target_y; + double target_z; + std::string source_frame_id; + double source_x; + double source_y; + double source_z; + std::string output_frame_id; + std::string input_frame_id; + bool use_camera_info; + double max_angular_rate; + double output_image_size; +}; + +class ImageRotateNode : public rclcpp::Node +{ +public: + IMAGE_ROTATE_PUBLIC ImageRotateNode(); + +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 std::string input_frame_from_msg); + void subscribe(); + void unsubscribe(); + void connectCb(); + void disconnectCb(); + void onInit(); + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_sub_; + std::shared_ptr tf_pub_; + + image_rotate::ImageRotateConfig config_; + + image_transport::Publisher img_pub_; + 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_; +}; +} // namespace image_rotate + +#endif // IMAGE_ROTATE__IMAGE_ROTATE_NODE_HPP_ diff --git a/image_rotate/include/image_rotate/visibility.h b/image_rotate/include/image_rotate/visibility.h new file mode 100644 index 000000000..3ef5050f1 --- /dev/null +++ b/image_rotate/include/image_rotate/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_ROTATE__VISIBILITY_H_ +#define IMAGE_ROTATE__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_ROTATE_EXPORT __attribute__ ((dllexport)) + #define IMAGE_ROTATE_IMPORT __attribute__ ((dllimport)) + #else + #define IMAGE_ROTATE_EXPORT __declspec(dllexport) + #define IMAGE_ROTATE_IMPORT __declspec(dllimport) + #endif + + #ifdef IMAGE_ROTATE_DLL + #define IMAGE_ROTATE_PUBLIC IMAGE_ROTATE_EXPORT + #else + #define IMAGE_ROTATE_PUBLIC IMAGE_ROTATE_IMPORT + #endif + + #define IMAGE_ROTATE_PUBLIC_TYPE IMAGE_ROTATE_PUBLIC + + #define IMAGE_ROTATE_LOCAL + +#else + + #define IMAGE_ROTATE_EXPORT __attribute__ ((visibility("default"))) + #define IMAGE_ROTATE_IMPORT + + #if __GNUC__ >= 4 + #define IMAGE_ROTATE_PUBLIC __attribute__ ((visibility("default"))) + #define IMAGE_ROTATE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define IMAGE_ROTATE_PUBLIC + #define IMAGE_ROTATE_LOCAL + #endif + + #define IMAGE_ROTATE_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // IMAGE_ROTATE__VISIBILITY_H_ diff --git a/image_rotate/nodelet_plugins.xml b/image_rotate/nodelet_plugins.xml deleted file mode 100644 index 862f136d9..000000000 --- a/image_rotate/nodelet_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Nodelet to rotate sensor_msgs/Image - - - diff --git a/image_rotate/package.xml b/image_rotate/package.xml index 74ac50084..22483dcd3 100644 --- a/image_rotate/package.xml +++ b/image_rotate/package.xml @@ -1,6 +1,6 @@ - + image_rotate - 1.12.23 + 2.0.0

Contains a node that rotates an image stream in a way that minimizes @@ -28,30 +28,22 @@ BSD http://ros.org/wiki/image_rotate - catkin + ament_cmake - rostest - - cmake_modules - cv_bridge - dynamic_reconfigure - geometry_msgs - image_transport - nodelet - roscpp - tf2 - tf2_geometry_msgs - tf2_ros + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + class_loader + + cv_bridge + image_transport + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros - cv_bridge - dynamic_reconfigure - image_transport - nodelet - roscpp - tf2 - tf2_geometry_msgs - tf2_ros - + ament_cmake diff --git a/image_rotate/src/image_rotate.cpp b/image_rotate/src/image_rotate.cpp new file mode 100644 index 000000000..da8e5dac7 --- /dev/null +++ b/image_rotate/src/image_rotate.cpp @@ -0,0 +1,54 @@ +// 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_rotate_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_rotate image_rotate image:="); + return 1; + } + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/image_rotate/src/image_rotate_node.cpp b/image_rotate/src/image_rotate_node.cpp new file mode 100644 index 000000000..01e779c5f --- /dev/null +++ b/image_rotate/src/image_rotate_node.cpp @@ -0,0 +1,345 @@ +// 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_rotate_node.cpp +* this is a forked version of image_rotate. +* this image_rotate_node supports: +* 1) node +* 2) tf and tf2 +*********************************************************************/ + +#include "image_rotate/image_rotate_node.hpp" + +#include +#include +#include + +namespace image_rotate +{ + +ImageRotateNode::ImageRotateNode() +: Node("ImageRotateNode") +{ + this->get_parameter_or_set("target_frame_id", config_.target_frame_id, std::string("")); + this->get_parameter_or_set("target_x", config_.target_x, static_cast(0)); + this->get_parameter_or_set("target_y", config_.target_y, static_cast(0)); + this->get_parameter_or_set("target_z", config_.target_z, static_cast(1)); + + this->get_parameter_or_set("source_frame_id", config_.source_frame_id, std::string("")); + this->get_parameter_or_set("source_x", config_.source_x, static_cast(0)); + this->get_parameter_or_set("source_y", config_.source_y, static_cast(-1)); + this->get_parameter_or_set("source_z", config_.source_z, static_cast(0)); + + this->get_parameter_or_set("output_frame_id", config_.output_frame_id, std::string("")); + this->get_parameter_or_set("input_frame_id", config_.input_frame_id, std::string("")); + this->get_parameter_or_set("use_camera_info", config_.use_camera_info, true); + this->get_parameter_or_set("max_angular_rate", config_.max_angular_rate, + static_cast(10)); + this->get_parameter_or_set("output_image_size", config_.output_image_size, + static_cast(2)); + + 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() == "target_x") { + config_.target_x = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset target_x as '%lf'", config_.target_x); + } else if (parameter.get_name() == "target_y") { + config_.target_y = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset target_y as '%lf'", config_.target_y); + } else if (parameter.get_name() == "target_z") { + config_.target_z = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset target_z as '%lf'", config_.target_z); + } else if (parameter.get_name() == "source_x") { + config_.source_x = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset source_x as '%lf'", config_.source_x); + } else if (parameter.get_name() == "source_y") { + config_.source_y = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset source_y as '%lf'", config_.source_y); + } else if (parameter.get_name() == "source_z") { + config_.source_z = parameter.as_double(); + RCLCPP_INFO(get_logger(), "Reset source_z as '%lf'", config_.source_z); + } + } + + target_vector_.vector.x = config_.target_x; + target_vector_.vector.y = config_.target_y; + target_vector_.vector.z = config_.target_z; + + source_vector_.vector.x = config_.source_x; + source_vector_.vector.y = config_.source_y; + source_vector_.vector.z = config_.source_z; + if (subscriber_count_) { // @todo: Could do this without an interruption at some point. + unsubscribe(); + subscribe(); + } + return result; + }; + this->register_param_change_callback(reconfigureCallback); + onInit(); +} + +const std::string ImageRotateNode::frameWithDefault( + const std::string & frame, + const std::string & image_frame) +{ + if (frame.empty()) { + return image_frame; + } + return frame; +} + +void ImageRotateNode::imageCallbackWithInfo( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info) +{ + do_work(msg, cam_info->header.frame_id); +} + +void ImageRotateNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + do_work(msg, msg->header.frame_id); +} + +void ImageRotateNode::do_work( + const sensor_msgs::msg::Image::ConstSharedPtr & msg, + const std::string input_frame_from_msg) +{ + try { + std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg); + std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg); + std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg); + + // Transform the target vector into the image frame. + target_vector_.header.stamp = msg->header.stamp; + target_vector_.header.frame_id = target_frame_id; + geometry_msgs::msg::Vector3Stamped target_vector_transformed; + tf2::TimePoint tf2_time = tf2_ros::fromMsg(msg->header.stamp); + + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + target_frame_id, input_frame_id, tf2_time, tf2_time - prev_stamp_); + tf2::doTransform(target_vector_, target_vector_transformed, transform); + + // Transform the source vector into the image frame. + source_vector_.header.stamp = msg->header.stamp; + source_vector_.header.frame_id = source_frame_id; + geometry_msgs::msg::Vector3Stamped source_vector_transformed; + transform = tf_buffer_->lookupTransform( + source_frame_id, input_frame_id, tf2_time, tf2_time - prev_stamp_); + tf2::doTransform(source_vector_, source_vector_transformed, transform); + + // RCUTILS_LOG_INFO("target: %f %f %f", target_vector_.x, target_vector_.y, target_vector_.z); + // RCUTILS_LOG_INFO("target_transformed: %f %f %f", target_vector_transformed.x, " + // "target_vector_transformed.y, target_vector_transformed.z"); + // RCUTILS_LOG_INFO("source: %f %f %f", source_vector_.x, source_vector_.y, source_vector_.z); + // RCUTILS_LOG_INFO("source_transformed: %f %f %f", source_vector_transformed.x, " + // "source_vector_transformed.y, source_vector_transformed.z"); + + // Calculate the angle of the rotation. + double angle = angle_; + if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) && + (source_vector_transformed.vector.x != 0 || source_vector_transformed.vector.y != 0)) + { + angle = atan2(target_vector_transformed.vector.y, target_vector_transformed.vector.x); + angle -= atan2(source_vector_transformed.vector.y, source_vector_transformed.vector.x); + } + + // Rate limit the rotation. + if (config_.max_angular_rate == 0) { + angle_ = angle; + } else { + double delta = fmod(angle - angle_, 2.0 * M_PI); + if (delta > M_PI) { + delta -= 2.0 * M_PI; + } else if (delta < -M_PI) { + delta += 2.0 * M_PI; + } + + double max_delta = config_.max_angular_rate * + (tf2_ros::timeToSec(msg->header.stamp) - tf2::timeToSec(prev_stamp_)); + if (delta > max_delta) { + delta = max_delta; + } else if (delta < -max_delta) { + delta = -max_delta; + } + + angle_ += delta; + } + angle_ = fmod(angle_, 2.0 * M_PI); + } catch (tf2::TransformException & e) { + RCUTILS_LOG_ERROR("Transform error: %s", e.what()); + } + + // RCUTILS_LOG_INFO("angle: %f", 180 * angle_ / M_PI); + + // Publish the transform. + geometry_msgs::msg::TransformStamped transform; + 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_)); + transform.header.frame_id = msg->header.frame_id; + transform.child_frame_id = frameWithDefault( + config_.output_frame_id, msg->header.frame_id + "_rotated"); + transform.header.stamp = msg->header.stamp; + + if (tf_pub_) { + tf_pub_->sendTransform(transform); + } + + // Transform the image. + try { + // Convert the image into something opencv can handle. + cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image; + + // Compute the output image size. + int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows; + int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows; + int noblack_dim = min_dim / sqrt(2); + int diag_dim = sqrt(in_image.cols * in_image.cols + in_image.rows * in_image.rows); + int out_size; + // diag_dim repeated to simplify limit case. + int candidates[] = {noblack_dim, min_dim, max_dim, diag_dim, diag_dim}; + int step = config_.output_image_size; + out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * + (config_.output_image_size - step); + // RCUTILS_LOG_INFO("out_size: %d", out_size); + + // Compute the rotation matrix. + cv::Mat rot_matrix = cv::getRotationMatrix2D( + cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1); + cv::Mat translation = rot_matrix.col(2); + rot_matrix.at(0, 2) += (out_size - in_image.cols) / 2.0; + rot_matrix.at(1, 2) += (out_size - in_image.rows) / 2.0; + + // Do the rotation + cv::Mat out_image; + cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size)); + + // 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; + img_pub_.publish(out_img); + } 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 ImageRotateNode::subscribe() +{ + RCUTILS_LOG_DEBUG("Subscribing to image topic."); + if (config_.use_camera_info && config_.input_frame_id.empty()) { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + + cam_sub_ = image_transport::create_camera_subscription( + this, + "image", + std::bind(&ImageRotateNode::imageCallbackWithInfo, this, + std::placeholders::_1, std::placeholders::_2), + "raw", + custom_qos); + } else { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + img_sub_ = image_transport::create_subscription( + this, + "image", + std::bind(&ImageRotateNode::imageCallback, this, std::placeholders::_1), + "raw", + custom_qos); + } +} + +void ImageRotateNode::unsubscribe() +{ + RCUTILS_LOG_DEBUG("Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); +} + +void ImageRotateNode::connectCb() +{ + if (subscriber_count_++ == 0) { + subscribe(); + } +} + +void ImageRotateNode::disconnectCb() +{ + subscriber_count_--; + if (subscriber_count_ == 0) { + unsubscribe(); + } +} + +void ImageRotateNode::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_); + // TODO(yechun1): Implement when SubscriberStatusCallback is available + // image_transport::SubscriberStatusCallback connect_cb = + // boost::bind(&ImageRotateNode::connectCb, this, _1); + // image_transport::SubscriberStatusCallback connect_cb = + // std::bind(&CropForemostNode::connectCb, this); + // image_transport::SubscriberStatusCallback disconnect_cb = + // boost::bind(&ImageRotateNode::disconnectCb, this, _1); + // img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise( + // "image", 1, connect_cb, disconnect_cb); + connectCb(); + img_pub_ = image_transport::create_publisher(this, "rotated/image"); + + // TODO(yechun1): cannot be used in reconstructor. Use different + // constructor once available. + tf_pub_ = std::make_shared(rclcpp::Node::SharedPtr(this)); +} +} // namespace image_rotate + +#include "class_loader/register_macro.hpp" + +// Register the component with class_loader. +CLASS_LOADER_REGISTER_CLASS(image_rotate::ImageRotateNode, rclcpp::Node) diff --git a/image_rotate/src/node/image_rotate.cpp b/image_rotate/src/node/image_rotate.cpp deleted file mode 100644 index d7e573b4f..000000000 --- a/image_rotate/src/node/image_rotate.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, 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 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 -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "image_rotate", ros::init_options::AnonymousName); - if (ros::names::remap("image") == "image") { - ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n" - "\t$ rosrun image_rotate image_rotate image:= [transport]"); - } - - nodelet::Loader manager(false); - nodelet::M_string remappings; - nodelet::V_string my_argv(argv + 1, argv + argc); - my_argv.push_back("--shutdown-on-close"); // Internal - - manager.load(ros::this_node::getName(), "image_rotate/image_rotate", remappings, my_argv); - - ros::spin(); - return 0; -} diff --git a/image_rotate/src/nodelet/image_rotate_nodelet.cpp b/image_rotate/src/nodelet/image_rotate_nodelet.cpp deleted file mode 100644 index 2fc023d7b..000000000 --- a/image_rotate/src/nodelet/image_rotate_nodelet.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2014, JSK Lab. -* 2008, 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 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_rotate_nodelet.cpp -* this is a forked version of image_rotate. -* this image_rotate_nodelet supports: -* 1) nodelet -* 2) tf and tf2 -*********************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace image_rotate { -class ImageRotateNodelet : public nodelet::Nodelet -{ - tf2_ros::Buffer tf_buffer_; - boost::shared_ptr tf_sub_; - tf2_ros::TransformBroadcaster tf_pub_; - - image_rotate::ImageRotateConfig config_; - dynamic_reconfigure::Server srv; - - image_transport::Publisher img_pub_; - image_transport::Subscriber img_sub_; - image_transport::CameraSubscriber cam_sub_; - - geometry_msgs::Vector3Stamped target_vector_; - geometry_msgs::Vector3Stamped source_vector_; - - boost::shared_ptr it_; - ros::NodeHandle nh_; - - int subscriber_count_; - double angle_; - ros::Time prev_stamp_; - - void reconfigureCallback(image_rotate::ImageRotateConfig &new_config, uint32_t level) - { - config_ = new_config; - target_vector_.vector.x = config_.target_x; - target_vector_.vector.y = config_.target_y; - target_vector_.vector.z = config_.target_z; - - source_vector_.vector.x = config_.source_x; - source_vector_.vector.y = config_.source_y; - source_vector_.vector.z = config_.source_z; - if (subscriber_count_) - { // @todo Could do this without an interruption at some point. - unsubscribe(); - subscribe(); - } - } - - const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) - { - if (frame.empty()) - return image_frame; - return frame; - } - - void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) - { - do_work(msg, cam_info->header.frame_id); - } - - void imageCallback(const sensor_msgs::ImageConstPtr& msg) - { - do_work(msg, msg->header.frame_id); - } - - void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg) - { - try - { - std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg); - std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg); - std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg); - - // Transform the target vector into the image frame. - target_vector_.header.stamp = msg->header.stamp; - target_vector_.header.frame_id = target_frame_id; - geometry_msgs::Vector3Stamped target_vector_transformed; - geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(target_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_); - tf2::doTransform(target_vector_, target_vector_transformed, transform); - - // Transform the source vector into the image frame. - source_vector_.header.stamp = msg->header.stamp; - source_vector_.header.frame_id = source_frame_id; - geometry_msgs::Vector3Stamped source_vector_transformed; - transform = tf_buffer_.lookupTransform(source_frame_id, input_frame_id, msg->header.stamp, msg->header.stamp-prev_stamp_); - tf2::doTransform(source_vector_, source_vector_transformed, transform); - - //NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z()); - //NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z()); - //NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z()); - //NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z()); - - // Calculate the angle of the rotation. - double angle = angle_; - if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) && - (source_vector_transformed.vector.x != 0 || source_vector_transformed.vector.y != 0)) - { - angle = atan2(target_vector_transformed.vector.y, target_vector_transformed.vector.x); - angle -= atan2(source_vector_transformed.vector.y, source_vector_transformed.vector.x); - } - - // Rate limit the rotation. - if (config_.max_angular_rate == 0) - angle_ = angle; - else - { - double delta = fmod(angle - angle_, 2.0 * M_PI); - if (delta > M_PI) - delta -= 2.0 * M_PI; - else if (delta < - M_PI) - delta += 2.0 * M_PI; - - double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec(); - if (delta > max_delta) - delta = max_delta; - else if (delta < -max_delta) - delta = - max_delta; - - angle_ += delta; - } - angle_ = fmod(angle_, 2.0 * M_PI); - } - catch (tf2::TransformException &e) - { - NODELET_ERROR("Transform error: %s", e.what()); - } - - //NODELET_INFO("angle: %f", 180 * angle_ / M_PI); - - // Publish the transform. - geometry_msgs::TransformStamped transform; - transform.transform.translation.x = 0; - transform.transform.translation.y = 0; - transform.transform.translation.z = 0; - tf2::convert(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_), transform.transform.rotation); - transform.header.frame_id = msg->header.frame_id; - transform.child_frame_id = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated"); - transform.header.stamp = msg->header.stamp; - tf_pub_.sendTransform(transform); - - // Transform the image. - try - { - // Convert the image into something opencv can handle. - cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image; - - // Compute the output image size. - int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows; - int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows; - int noblack_dim = min_dim / sqrt(2); - int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows); - int out_size; - int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case. - int step = config_.output_image_size; - out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step); - //NODELET_INFO("out_size: %d", out_size); - - // Compute the rotation matrix. - cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1); - cv::Mat translation = rot_matrix.col(2); - rot_matrix.at(0, 2) += (out_size - in_image.cols) / 2.0; - rot_matrix.at(1, 2) += (out_size - in_image.rows) / 2.0; - - // Do the rotation - cv::Mat out_image; - cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size)); - - // Publish the image. - sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg(); - out_img->header.frame_id = transform.child_frame_id; - img_pub_.publish(out_img); - } - catch (cv::Exception &e) - { - NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); - } - - prev_stamp_ = msg->header.stamp; - } - - void subscribe() - { - NODELET_DEBUG("Subscribing to image topic."); - if (config_.use_camera_info && config_.input_frame_id.empty()) - cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this); - else - img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this); - } - - void unsubscribe() - { - NODELET_DEBUG("Unsubscribing from image topic."); - img_sub_.shutdown(); - cam_sub_.shutdown(); - } - - void connectCb(const image_transport::SingleSubscriberPublisher& ssp) - { - if (subscriber_count_++ == 0) { - subscribe(); - } - } - - void disconnectCb(const image_transport::SingleSubscriberPublisher&) - { - subscriber_count_--; - if (subscriber_count_ == 0) { - unsubscribe(); - } - } - -public: - virtual void onInit() - { - nh_ = getNodeHandle(); - it_ = boost::shared_ptr(new image_transport::ImageTransport(nh_)); - subscriber_count_ = 0; - angle_ = 0; - prev_stamp_ = ros::Time::now(); - tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_)); - image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ImageRotateNodelet::connectCb, this, _1); - image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1); - img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb); - - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2); - srv.setCallback(f); - } -}; -} -#include -PLUGINLIB_EXPORT_CLASS(image_rotate::ImageRotateNodelet, nodelet::Nodelet);