Merge branch 'ros-perception:rolling' into rolling

quasi-robotics · Dec 13, 2024 · 5241c4d · 5241c4d
2 parents 7fdb3cd + afc34b4
commit 5241c4d
Show file tree

Hide file tree

Showing 30 changed files with 572 additions and 22 deletions.
diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package camera_calibration
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+
 6.0.5 (2024-10-30)
 ------------------
 

diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>camera_calibration</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>
      camera_calibration allows easy calibration of monocular or stereo
      cameras using a checkerboard calibration target.

diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py
@@ -5,7 +5,7 @@
 
 setup(
     name=PACKAGE_NAME,
-    version='6.0.5',
+    version='6.0.6',
     packages=["camera_calibration", "camera_calibration.nodes"],
     data_files=[
         ('share/ament_index/resource_index/packages',

diff --git a/depth_image_proc/CHANGELOG.rst b/depth_image_proc/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package depth_image_proc
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+* Support QoS override parameters in depth_image_proc/register (`#1043 <https://github.com/ros-perception/image_pipeline/issues/1043>`_)
+  This PR adds support to the `depth_image_proc` - `register` node for
+  setting External QoS Configuration on topic _subscriptions\_.
+* Contributors: Stuart Alldritt
+
 6.0.5 (2024-10-30)
 ------------------
 

diff --git a/depth_image_proc/package.xml b/depth_image_proc/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>depth_image_proc</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>
 
      Contains components for processing depth images such as those

diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp
@@ -133,14 +133,19 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
         sub_depth_info_.unsubscribe();
         sub_rgb_info_.unsubscribe();
       } else if (!sub_depth_image_.getSubscriber()) {
+        // Allow overriding QoS settings (history, depth, reliability)
+        rclcpp::SubscriptionOptions sub_options;
+        sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+
         // For compressed topics to remap appropriately, we need to pass a
         // fully expanded and remapped topic name to image_transport
         auto node_base = this->get_node_base_interface();
         std::string topic = node_base->resolve_topic_or_service_name("depth/image_rect", false);
         image_transport::TransportHints hints(this, "raw", "depth_image_transport");
-        sub_depth_image_.subscribe(this, topic, hints.getTransport());
-        sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10));
-        sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10));
+        sub_depth_image_.subscribe(this, topic, hints.getTransport(), rmw_qos_profile_default,
+          sub_options);
+        sub_depth_info_.subscribe(this, "depth/camera_info", rclcpp::QoS(10), sub_options);
+        sub_rgb_info_.subscribe(this, "rgb/camera_info", rclcpp::QoS(10), sub_options);
       }
     };
   // For compressed topics to remap appropriately, we need to pass a

diff --git a/image_pipeline/CHANGELOG.rst b/image_pipeline/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package image_pipeline
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+
 6.0.5 (2024-10-30)
 ------------------
 

diff --git a/image_pipeline/package.xml b/image_pipeline/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>image_pipeline</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>image_pipeline fills the gap between getting raw images from a camera driver and higher-level vision processing.</description>
 
   <maintainer email="[email protected]">Vincent Rabaud</maintainer>

diff --git a/image_proc/CHANGELOG.rst b/image_proc/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package image_proc
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+
 6.0.5 (2024-10-30)
 ------------------
 * Use TF2 package for quaternion conversion (`#1031 <https://github.com/ros-perception/image_pipeline/issues/1031>`_)

diff --git a/image_proc/package.xml b/image_proc/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>image_proc</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>Single image rectification and color processing.</description>
 
   <maintainer email="[email protected]">Vincent Rabaud</maintainer>

diff --git a/image_publisher/CHANGELOG.rst b/image_publisher/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package image_publisher
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+
 6.0.5 (2024-10-30)
 ------------------
 

diff --git a/image_publisher/package.xml b/image_publisher/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>image_publisher</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>
 
       Contains a node publish an image stream from single image file

diff --git a/image_rotate/CHANGELOG.rst b/image_rotate/CHANGELOG.rst
@@ -2,6 +2,22 @@
 Changelog for package image_rotate
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.6 (2024-12-11)
+------------------
+* add image_flip node (`#942 <https://github.com/ros-perception/image_pipeline/issues/942>`_)
+  This is a continuation of
+  https://github.com/ros-perception/image_pipeline/pull/756:
+  * [x] Squashed 16 commits in original PR for ease of rebase/review
+  * [x] Moved node into image_rotate package
+  * [x] Added lazy subscriber
+  * [x] Removes QoS parameters - will add proper QoS overrides in a
+  different PR (when we do the same for image_rotate)
+  * [x] Adds documentation
+  ---------
+  Co-authored-by: David Conner <[email protected]>
+  Co-authored-by: Alejandro Hernández Cordero <[email protected]>
+* Contributors: Michael Ferguson
+
 6.0.5 (2024-10-30)
 ------------------
 

diff --git 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
@@ -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
@@ -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 <math.h>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <memory>
+#include <string>
+
+#include <cv_bridge/cv_bridge.hpp>
+#include <image_transport/image_transport.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#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<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_sub_;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> 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
@@ -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
@@ -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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>image_rotate</name>
-  <version>6.0.5</version>
+  <version>6.0.6</version>
   <description>
     <p>
       Contains a node that rotates an image stream in a way that minimizes
@@ -34,6 +34,7 @@
   <url type="bugtracker">https://github.com/ros-perception/image_pipeline/issues</url>
   <url type="repository">https://github.com/ros-perception/image_pipeline</url>
   <author>Blaise Gassend</author>
+  <author email="[email protected]">David Conner</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>