Merge pull request #136 from introlab/ros2

Ros2 (Addressing #131 #107)

.github/workflows/cmake.yml

@@ -0,0 +1,36 @@
+name: CMake
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    name: ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04]
+
+    steps:    
+    - name: Install dependencies
+      run: |
+        DEBIAN_FRONTEND=noninteractive
+        sudo apt-get update
+        sudo apt-get -y install libopencv-dev qtbase5-dev git cmake software-properties-common
+
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+      

.github/workflows/ros1.yml

@@ -0,0 +1,59 @@
+name: ros1
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-18.04]
+        include:
+        - os: ubuntu-20.04
+          ros_distro: 'noetic'
+        - os: ubuntu-18.04
+          ros_distro: 'melodic'
+
+    steps:   
+    - uses: ros-tooling/setup-ros@v0.2
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge qtbase5-dev
+        
+    - name: Setup catkin workspace
+      run: | 
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        mkdir -p ${{github.workspace}}/catkin_ws/src
+        cd ${{github.workspace}}/catkin_ws/src
+        catkin_init_workspace
+        cd ..
+        catkin_make
+        
+    - uses: actions/checkout@v2
+      with:
+        path: 'catkin_ws/src/find_object_2d'
+        
+    - name: caktkin_make
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source ${{github.workspace}}/catkin_ws/devel/setup.bash
+        cd ${{github.workspace}}/catkin_ws
+        catkin_make -DSETUPTOOLS_DEB_LAYOUT=OFF
+        catkin_make install     

.github/workflows/ros2.yml

@@ -0,0 +1,58 @@
+
+name: ros2
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        ros_distro: [foxy, galactic, humble]
+        include:
+        - ros_distro: 'foxy'
+          os: ubuntu-20.04
+        - ros_distro: 'galactic'
+          os: ubuntu-20.04
+        - ros_distro: 'humble'
+          os: ubuntu-22.04
+
+    steps:   
+    - uses: ros-tooling/setup-ros@v0.4
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge qtbase5-dev
+                
+    - name: Setup ros2 workspace
+      run: | 
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        mkdir -p ${{github.workspace}}/ros2_ws/src
+        cd ${{github.workspace}}/ros2_ws
+        colcon build
+        
+    - uses: actions/checkout@v2
+      with:
+        path: 'ros2_ws/src/find_object_2d'
+        
+    - name: colcon build
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        cd ${{github.workspace}}/ros2_ws
+        colcon build --event-handlers console_direct+

CMakeLists.txt

@@ -1,18 +1,33 @@
-cmake_minimum_required(VERSION 2.8.5)
+cmake_minimum_required(VERSION 3.5)
 
-# Detect if it is called by catkin
+SET(CATKIN_BUILD FALSE)
+SET(COLCON_BUILD FALSE)
+
+# Detect if it is called by catkin (ros1)
 IF(CATKIN_TOPLEVEL OR CATKIN_BUILD_BINARY_PACKAGE OR CATKIN_SKIP_TESTING OR CATKIN_ENABLE_TESTING OR CATKIN_DEVEL_PREFIX)
   SET(CATKIN_BUILD TRUE)
-ENDIF(CATKIN_TOPLEVEL OR CATKIN_BUILD_BINARY_PACKAGE OR CATKIN_SKIP_TESTING OR CATKIN_ENABLE_TESTING OR CATKIN_DEVEL_PREFIX)
+ELSE()
+  # Detect if it is called by colcon (ros2)
+  string(FIND ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR} POS)
+  IF(${POS} EQUAL -1)
+     SET(COLCON_BUILD TRUE)
+  ENDIF()
+ENDIF()
 
 MESSAGE(STATUS "CATKIN_BUILD=${CATKIN_BUILD}")
+MESSAGE(STATUS "COLCON_BUILD=${COLCON_BUILD}")
+SET(ROS_BUILD FALSE)
+IF(CATKIN_BUILD OR COLCON_BUILD)
+  SET(ROS_BUILD TRUE)
+ENDIF()
 
-IF(NOT CATKIN_BUILD)
+IF(NOT ROS_BUILD)
    #Standalone build
    PROJECT( Find-Object )
 ELSE()
-   #ROS catkin build
+   #ROS build
    PROJECT( find_object_2d )
+   MESSAGE(STATUS "ROS_DISTRO=$ENV{ROS_DISTRO}")
 ENDIF()
 
 # Catkin doesn't support multiarch library path, 
@@ -130,7 +145,7 @@ ENDIF()
 
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/include/${PROJECT_PREFIX}/Version.h)
 
-IF(NOT CATKIN_BUILD)
+IF(NOT ROS_BUILD)
    #Standalone build
    IF(WIN32 AND NOT MINGW)
        ADD_DEFINITIONS("-wd4251")
@@ -358,14 +373,14 @@ IF(NOT CATKIN_BUILD)
    ENDIF(APPLE)
    MESSAGE(STATUS "--------------------------------------------")
 
-ELSE()
+ELSEIF(CATKIN_BUILD)
    #ROS Catkin build
 
    ## Find catkin macros and libraries
    ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
    ## is used, also find other catkin packages
    find_package(catkin REQUIRED COMPONENTS 
-           cv_bridge roscpp rospy sensor_msgs std_msgs image_transport message_filters tf message_generation
+           cv_bridge roscpp sensor_msgs std_msgs image_transport message_filters tf message_generation
    )
 
    ## Generate messages in the 'msg' folder
@@ -392,7 +407,7 @@ ELSE()
    ## CATKIN_DEPENDS: catkin_packages dependent projects also need
    ## DEPENDS: system dependencies of this project that dependent projects also need
    catkin_package(
-     CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs image_transport message_filters tf message_runtime
+     CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs image_transport message_filters tf message_runtime
      DEPENDS OpenCV
    )
 
@@ -405,12 +420,49 @@ ELSE()
    ## Install ##
    #############
    ## Mark other files for installation (e.g. launch and bag files, etc.)
-   install(FILES
+   install(DIRECTORY
-      launch/find_object_3d_kinect2.launch
+      launch/ros1
-      launch/find_object_3d_zed.launch
-      launch/find_object_2d.launch
-      launch/find_object_3d.launch
       DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
    )
+ELSE() # COLCON_BUILD
+   #ROS Colcon build
+
+   find_package(ament_cmake REQUIRED)
+   find_package(builtin_interfaces REQUIRED)
+   find_package(rosidl_default_generators REQUIRED)
+   find_package(rclcpp REQUIRED)
+   find_package(rclcpp_components REQUIRED)
+   find_package(cv_bridge REQUIRED)
+   find_package(sensor_msgs REQUIRED)
+   find_package(std_msgs REQUIRED)
+   find_package(image_transport REQUIRED)
+   find_package(message_filters REQUIRED)
+   find_package(tf2 REQUIRED)
+   find_package(tf2_ros REQUIRED)
+   find_package(tf2_geometry_msgs REQUIRED)
+   find_package(geometry_msgs REQUIRED)
+
+   ## Generate messages and services
+   rosidl_generate_interfaces(${PROJECT_NAME}
+      msg/ObjectsStamped.msg
+      msg/DetectionInfo.msg
+      DEPENDENCIES std_msgs sensor_msgs
+   )
+   ament_export_dependencies(rosidl_default_runtime)
+
+   ###########
+   ## Build ##
+   ###########
+   ADD_SUBDIRECTORY( src )
+
+   #############
+   ## Install ##
+   #############
+   install(DIRECTORY 
+      launch/ros2/.
+      DESTINATION share/${PROJECT_NAME}/launch
+   )
+
+   ament_package()
 ENDIF()
 

README.md

@@ -1,48 +1,92 @@
-## find-object (standalone) 
+# find-object
-Linux: [![Build Status](https://travis-ci.com/introlab/find-object.svg?branch=master)](https://travis-ci.com/introlab/find-object) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hn51r6p5c0peqctb/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/find-object/branch/master)
 
+<table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/find-object/actions/workflows/cmake.yml"><img src="https://github.com/introlab/find-object/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/find-object/actions/workflows/ros1.yml"><img src="https://github.com/introlab/find-object/actions/workflows/ros1.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/find-object/actions/workflows/ros2.yml"><img src="https://github.com/introlab/find-object/actions/workflows/ros2.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/find-object/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hn51r6p5c0peqctb/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+
+## Standalone
 Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information.
 
-## find_object_2d (ROS package)
+## ROS1
 
 ### Install
 
 Binaries:
 ```bash
-# ROS Kinetic:
+sudo apt-get install ros-$ROS_DISTRO-find-object-2d
- $ sudo apt-get install ros-kinetic-find-object-2d
-# ROS Jade:
- $ sudo apt-get install ros-jade-find-object-2d
-# ROS Indigo:
- $ sudo apt-get install ros-indigo-find-object-2d
-# ROS Hydro:
- $ sudo apt-get install ros-hydro-find-object-2d
 ```
 
 Source:
 
- * If you want SURF/SIFT on Indigo/Jade/Kinetic (Hydro has already SIFT/SURF), you have to build [OpenCV](http://opencv.org/) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the Find-Object should link with it instead of the one installed in ROS.
+ * To include `xfeatures2d` and/or `nonfree` modules of OpenCV, to avoid conflicts with `cv_bridge`, build same OpenCV version that is used by `cv_bridge`. Install it in `/usr/local` (default).
-
-     * On Indigo/Jade, I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). Find-Object can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but find_object_2d package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3.
-
-     * On Kinetic, I recommend to use OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module (*to confirm* OpenCV3 installed with ROS already includes SIFT/SURF, so no need to rebuild OpenCV). You can also install OpenCV2, but find_object_2d package will have libraries conflict as cv-bridge is depending on OpenCV3. Thus if you want OpenCV2 on Kinetic, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV2.
 
 ```bash
-# Install ROS Groovy/Hydro/Indigo/Jade/Kinetic (catkin build):
+cd ~/catkin_ws
- $ cd ~/catkin_ws
+git clone https://github.com/introlab/find-object.git src/find_object_2d
- $ git clone https://github.com/introlab/find-object.git src/find_object_2d
+catkin_make
- $ catkin_make
-
-# Install ROS Fuerte (in a directory of your "ROS_PACKAGE_PATH"):
- $ svn checkout -r176 http://find-object.googlecode.com/svn/trunk/ros-pkg/find_object_2d
- $ rosmake find_object_2d
 ```
 
 ### Run
 ```bash
- $ roscore &
+roscore
- # Launch your preferred usb camera driver
+# Launch your preferred usb camera driver
- $ rosrun uvc_camera uvc_camera_node &
+rosrun uvc_camera uvc_camera_node
- $ rosrun find_object_2d find_object_2d image:=image_raw
+rosrun find_object_2d find_object_2d image:=image_raw
 ```
 See [find_object_2d](http://wiki.ros.org/find_object_2d) for more information.
+
+## ROS2
+
+### Install
+
+Binaries:
+```bash
+To come...
+```
+
+Source:
+
+```bash
+cd ~/ros2_ws
+git clone https://github.com/introlab/find-object.git src/find_object_2d
+colcon build
+```
+
+### Run
+```bash
+# Launch your preferred usb camera driver
+ros2 launch realsense2_camera rs_launch.py
+ 
+# Launch find_object_2d node:
+ros2 launch find_object_2d find_object_2d.launch.py image:=/camera/color/image_raw
+ 
+# Draw objects detected on an image:
+ros2 run find_object_2d print_objects_detected --ros-args -r image:=/camera/color/image_raw
+```
+#### 3D Pose (TF)
+A RGB-D camera is required. Example with Realsense D400 camera:
+```bash
+# Launch your preferred usb camera driver
+ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
+ 
+# Launch find_object_2d node:
+ros2 launch find_object_2d find_object_3d.launch.py \
+   rgb_topic:=/camera/color/image_raw \
+   depth_topic:=/camera/aligned_depth_to_color/image_raw \
+   camera_info_topic:=/camera/color/camera_info
+ 
+# Show 3D pose in camera frame:
+ros2 run find_object_2d tf_example
+```
+

launch/ros2/find_object_2d.launch.py

@@ -0,0 +1,30 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+   
+    return LaunchDescription([
+
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '0'),
+
+        # Launch arguments
+        DeclareLaunchArgument('gui', default_value='true', description='Launch GUI.'),
+        DeclareLaunchArgument('image_topic', default_value='image', description='Image topic to subscribe to.'),
+        DeclareLaunchArgument('objects_path', default_value='~/objects', description='Directory containing objects to load on initialization.'),
+        DeclareLaunchArgument('settings_path', default_value='~/.ros/find_object_2d.ini', description='Config file.'),
+
+        # Nodes to launch
+        Node(
+            package='find_object_2d', executable='find_object_2d', output='screen',
+            parameters=[{
+              'gui':LaunchConfiguration('gui'),
+              'objects_path':LaunchConfiguration('objects_path'),
+              'settings_path':LaunchConfiguration('settings_path')
+            }],
+            remappings=[
+                ('image', LaunchConfiguration('image_topic'))]), 
+    ])

launch/ros2/find_object_3d.launch.py

@@ -0,0 +1,42 @@
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+   
+    return LaunchDescription([
+
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '0'),
+
+        # Launch arguments
+        DeclareLaunchArgument('gui', default_value='true', description='Launch GUI.'),
+        DeclareLaunchArgument('approx_sync', default_value='true', description=''),
+        DeclareLaunchArgument('pnp', default_value='true', description=''),
+        DeclareLaunchArgument('object_prefix', default_value='object', description='TF prefix of objects.'),
+        DeclareLaunchArgument('objects_path', default_value='~/objects', description='Directory containing objects to load on initialization.'),
+        DeclareLaunchArgument('settings_path', default_value='~/.ros/find_object_2d.ini', description='Config file.'),
+
+        DeclareLaunchArgument('rgb_topic', default_value='camera/rgb/image_rect_color', description='Image topic.'),
+        DeclareLaunchArgument('depth_topic', default_value='camera/depth_registered/image_raw', description='Registered depth topic.'),
+        DeclareLaunchArgument('camera_info_topic', default_value='camera/rgb/camera_info', description='Camera info topic.'),
+
+        # Nodes to launch
+        Node(
+            package='find_object_2d', executable='find_object_2d', output='screen',
+            parameters=[{
+              'subscribe_depth':True,
+              'gui':LaunchConfiguration('gui'),
+              'approx_sync':LaunchConfiguration('approx_sync'),
+              'pnp':LaunchConfiguration('pnp'),
+              'object_prefix':LaunchConfiguration('object_prefix'),
+              'objects_path':LaunchConfiguration('objects_path'),
+              'settings_path':LaunchConfiguration('settings_path')
+            }],
+            remappings=[
+                ('rgb/image_rect_color', LaunchConfiguration('rgb_topic')),
+                ('depth_registered/image_raw', LaunchConfiguration('depth_topic')),
+                ('depth_registered/camera_info', LaunchConfiguration('camera_info_topic'))]), 
+    ])

msg/DetectionInfo.msg

@@ -1,11 +1,11 @@
 
-Header header
+std_msgs/Header header
 
 # All arrays should have the same size
 std_msgs/Int32[] ids
 std_msgs/Int32[] widths
 std_msgs/Int32[] heights
-std_msgs/String[] filePaths
+std_msgs/String[] file_paths
 std_msgs/Int32[] inliers
 std_msgs/Int32[] outliers
 # 3x3 homography matrix: [h11, h12, h13, h21, h22, h23, h31, h32, h33] (h31 = dx and h32 = dy, see QTransform)

msg/ObjectsStamped.msg

@@ -1,5 +1,5 @@
 # objects format: 
 # [ObjectId1, objectWidth, objectHeight, h11, h12, h13, h21, h22, h23, h31, h32, h33, ObjectId2...] 
 # where h## is a 3x3 homography matrix (h31 = dx and h32 = dy, see QTransform)
-Header header
+std_msgs/Header header
 std_msgs/Float32MultiArray objects 

package.xml

@@ -1,38 +1,52 @@
 <?xml version="1.0"?>
-<package>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>find_object_2d</name>
   <version>0.6.4</version>
   <description>The find_object_2d package</description>
-
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
   <license>BSD</license>
-  <url type="website">http://find-object.googlecode.com</url>
+  <url type="bugtracker">https://github.com/introlab/find-object/issues</url>
-  <author email="matlabbe@gmail.com">Mathieu Labbe</author>
+  <url type="repository">https://github.com/introlab/find-object</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
   
-  <build_depend>message_generation</build_depend>
+  <depend condition="$ROS_VERSION == 1">roscpp</depend>
+  <depend condition="$ROS_VERSION == 1">tf</depend>
+  <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+  <depend condition="$ROS_VERSION == 2">tf2</depend>
+  <depend condition="$ROS_VERSION == 2">tf2_ros</depend>
+  <depend condition="$ROS_VERSION == 2">tf2_geometry_msgs</depend>
+  
+  <build_depend    condition="$ROS_VERSION == 2">ros_environment</build_depend>
+  <depend          condition="$ROS_VERSION == 2">builtin_interfaces</depend>
+  <build_depend    condition="$ROS_VERSION == 2">rosidl_default_generators</build_depend>
+  <exec_depend     condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
+  <member_of_group condition="$ROS_VERSION == 2">rosidl_interface_packages</member_of_group>
+ 
+  <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
   <build_depend>qtbase5-dev</build_depend>
   <build_depend>cv_bridge</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>message_filters</build_depend>
-  <build_depend>tf</build_depend>
   
-  <run_depend>message_runtime</run_depend>
+  <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
-  <run_depend>qtbase5-dev</run_depend>
+  <exec_depend>qtbase5-dev</exec_depend>
-  <run_depend>cv_bridge</run_depend>
+  <exec_depend>cv_bridge</exec_depend>
-  <run_depend>roscpp</run_depend>
+  <exec_depend>sensor_msgs</exec_depend>
-  <run_depend>rospy</run_depend>
+  <exec_depend>std_msgs</exec_depend>
-  <run_depend>sensor_msgs</run_depend>
+  <exec_depend>std_srvs</exec_depend>
-  <run_depend>std_msgs</run_depend>
+  <exec_depend>geometry_msgs</exec_depend>
-  <run_depend>std_srvs</run_depend>
+  <exec_depend>image_transport</exec_depend>
-  <run_depend>image_transport</run_depend>
+  <exec_depend>message_filters</exec_depend>
-  <run_depend>message_filters</run_depend>
-  <run_depend>tf</run_depend>
   
+  <export>
+	<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
 </package>

src/CMakeLists.txt

@@ -19,10 +19,17 @@ SET(headers_ui
 IF(CATKIN_BUILD)
    SET(headers_ui  
       ${headers_ui}
-      ./ros/CameraROS.h
+      ./ros1/CameraROS.h
-      ./ros/FindObjectROS.h
+      ./ros1/FindObjectROS.h
    )
 ENDIF(CATKIN_BUILD)
+IF(COLCON_BUILD)
+   SET(headers_ui  
+      ${headers_ui}
+      ./ros2/CameraROS.h
+      ./ros2/FindObjectROS.h
+   )
+ENDIF(COLCON_BUILD)
 
 SET(uis
    ./ui/mainWindow.ui
@@ -82,12 +89,17 @@ SET(SRC_FILES
 IF(CATKIN_BUILD)
    SET(SRC_FILES 
       ${SRC_FILES}
-      ./ros/CameraROS.cpp
+      ./ros1/CameraROS.cpp
-      ./ros/FindObjectROS.cpp
+      ./ros1/FindObjectROS.cpp
    )
 ENDIF(CATKIN_BUILD)
-
+IF(COLCON_BUILD)
-
+   SET(SRC_FILES 
+      ${SRC_FILES}
+      ./ros2/CameraROS.cpp
+      ./ros2/FindObjectROS.cpp
+   )
+ENDIF(COLCON_BUILD)
 
 SET(INCLUDE_DIRS
    ${CMAKE_CURRENT_SOURCE_DIR}/../include
@@ -112,6 +124,22 @@ IF(CATKIN_BUILD)
       ${catkin_LIBRARIES}
    )
 ENDIF(CATKIN_BUILD)
+IF(COLCON_BUILD)
+   SET(AMENT_LIBRARIES
+      rclcpp
+      rclcpp_components
+      cv_bridge
+      sensor_msgs
+      std_msgs
+      image_transport
+      message_filters
+      tf2
+      tf2_ros
+      tf2_geometry_msgs
+      geometry_msgs
+   )
+ENDIF(COLCON_BUILD)
+
 
 IF(TORCH_FOUND)
 	SET(LIBRARIES
@@ -141,31 +169,51 @@ ENDIF(CATKIN_BUILD)
 # create a library from the source files
 ADD_LIBRARY(find_object ${SRC_FILES})
 # Linking with Qt libraries
-TARGET_LINK_LIBRARIES(find_object ${LIBRARIES})
 IF(Qt5_FOUND)
     QT5_USE_MODULES(find_object Widgets Core Gui Network PrintSupport)
 ENDIF(Qt5_FOUND)
+
+IF(NOT COLCON_BUILD)
+   TARGET_LINK_LIBRARIES(find_object ${LIBRARIES})
+ENDIF()
 IF(CATKIN_BUILD)
    set_target_properties(find_object PROPERTIES  OUTPUT_NAME find_object_2d)
    add_dependencies(find_object ${${PROJECT_NAME}_EXPORTED_TARGETS})
 ENDIF(CATKIN_BUILD)
+IF(COLCON_BUILD)
+   ament_target_dependencies(find_object ${AMENT_LIBRARIES})
    
-IF(NOT CATKIN_BUILD)
+   if("$ENV{ROS_DISTRO}" STRGREATER_EQUAL "humble")
+      rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+      target_link_libraries(find_object ${cpp_typesupport_target} ${LIBRARIES})
+   else()
+      # foxy, galatic
+      target_link_libraries(find_object ${LIBRARIES})
+      rosidl_target_interfaces(find_object ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+      function(rosidl_get_typesupport_target var generate_interfaces_target typesupport_name)
+         rosidl_target_interfaces(${var} ${generate_interfaces_target} ${typesupport_name})
+      endfunction()
+      add_definitions(-DPRE_ROS_HUMBLE)
+   endif()
+ENDIF(COLCON_BUILD)
+
+IF(NOT ROS_BUILD)
    INSTALL(TARGETS find_object
       RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
       LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel
       ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel)
       
    install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
-ELSE()
+ELSEIF(CATKIN_BUILD)
-   add_executable(find_object_2d ros/find_object_2d_node.cpp)
+   add_executable(find_object_2d ros1/find_object_2d_node.cpp)
    target_link_libraries(find_object_2d find_object ${LIBRARIES})
 
-   add_executable(print_objects_detected ros/print_objects_detected_node.cpp)
+   add_executable(print_objects_detected ros1/print_objects_detected_node.cpp)
    target_link_libraries(print_objects_detected ${LIBRARIES})
    add_dependencies(print_objects_detected ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
-   add_executable(tf_example ros/tf_example_node.cpp)
+   add_executable(tf_example ros1/tf_example_node.cpp)
    target_link_libraries(tf_example ${LIBRARIES})
    add_dependencies(tf_example ${${PROJECT_NAME}_EXPORTED_TARGETS})
    
@@ -185,4 +233,49 @@ ELSE()
       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
     )
+ELSE() # COLCON_BUILD
+   add_executable(find_object_2d_node ros2/find_object_2d_node.cpp)
+   set_target_properties(find_object_2d_node PROPERTIES  OUTPUT_NAME find_object_2d)
+   ament_target_dependencies(find_object_2d_node ${AMENT_LIBRARIES})
+   target_link_libraries(find_object_2d_node find_object ${LIBRARIES})
+   
+   add_executable(print_objects_detected ros2/print_objects_detected_node.cpp)
+   ament_target_dependencies(print_objects_detected ${AMENT_LIBRARIES})
+   target_link_libraries(print_objects_detected find_object ${LIBRARIES})
+   
+   add_executable(tf_example ros2/tf_example_node.cpp)
+   ament_target_dependencies(tf_example ${AMENT_LIBRARIES})
+   target_link_libraries(tf_example find_object ${LIBRARIES})
+   
+   # Only required when using messages built from the same package
+   # https://index.ros.org/doc/ros2/Tutorials/Rosidl-Tutorial/
+   get_default_rmw_implementation(rmw_implementation)
+   find_package("${rmw_implementation}" REQUIRED)
+   get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp")
+
+   foreach(typesupport_impl ${typesupport_impls})
+      rosidl_get_typesupport_target(find_object_2d_node
+        ${PROJECT_NAME} ${typesupport_impl}
+      )
+      rosidl_get_typesupport_target(print_objects_detected
+        ${PROJECT_NAME} ${typesupport_impl}
+      )
+      rosidl_get_typesupport_target(tf_example
+        ${PROJECT_NAME} ${typesupport_impl}
+      )
+   endforeach()
+   
+   ## Mark executables and/or libraries for installation
+   install(TARGETS 
+      find_object
+      ARCHIVE DESTINATION lib
+      LIBRARY DESTINATION lib
+      RUNTIME DESTINATION bin
+   )
+   install(TARGETS 
+      find_object_2d_node
+      print_objects_detected
+      tf_example
+      DESTINATION lib/${PROJECT_NAME}
+   )
 ENDIF()

src/ros1/CameraROS.cpp

@@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CameraROS.h"
 #include "find_object/Settings.h"
 
 #include <opencv2/imgproc/imgproc.hpp>
+#include "CameraROS.h"
 #include <sensor_msgs/image_encodings.h>
 
 using namespace find_object;

src/ros1/FindObjectROS.cpp

@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "FindObjectROS.h"
-
 #include <std_msgs/Float32MultiArray.h>
 #include "find_object_2d/ObjectsStamped.h"
 #include "find_object_2d/DetectionInfo.h"
@@ -209,7 +208,7 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const Heade
 			infoMsg.ids.resize(info.objDetected_.size());
 			infoMsg.widths.resize(info.objDetected_.size());
 			infoMsg.heights.resize(info.objDetected_.size());
-			infoMsg.filePaths.resize(info.objDetected_.size());
+			infoMsg.file_paths.resize(info.objDetected_.size());
 			infoMsg.inliers.resize(info.objDetected_.size());
 			infoMsg.outliers.resize(info.objDetected_.size());
 			infoMsg.homographies.resize(info.objDetected_.size());
@@ -253,7 +252,7 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const Heade
 				infoMsg.ids[infoIndex].data = iter.key();
 				infoMsg.widths[infoIndex].data = iterSizes->width();
 				infoMsg.heights[infoIndex].data = iterSizes->height();
-				infoMsg.filePaths[infoIndex].data = iterFilePaths.value().toStdString();
+				infoMsg.file_paths[infoIndex].data = iterFilePaths.value().toStdString();
 				infoMsg.inliers[infoIndex].data = iterInliers.value();
 				infoMsg.outliers[infoIndex].data = iterOutliers.value();
 				infoMsg.homographies[infoIndex].data.resize(9);

src/ros1/find_object_2d_node.cpp

@@ -25,9 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CameraROS.h"
+#include <ros1/CameraROS.h>
-#include "FindObjectROS.h"
+#include <ros1/FindObjectROS.h>
-
 #include <QApplication>
 #include <QDir>
 #include "find_object/MainWindow.h"

src/ros2/CameraROS.cpp

@@ -0,0 +1,188 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 "CameraROS.h"
+#include "find_object/Settings.h"
+
+#include <opencv2/imgproc/imgproc.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+
+using namespace find_object;
+
+CameraROS::CameraROS(bool subscribeDepth, rclcpp::Node * node) :
+	node_(node),
+	subscribeDepth_(subscribeDepth),
+	approxSync_(0),
+	exactSync_(0)
+{
+	qRegisterMetaType<rclcpp::Time>("ros::Time");
+	qRegisterMetaType<cv::Mat>("cv::Mat");
+
+	if(!subscribeDepth_)
+	{
+		image_transport::TransportHints hints(node);
+		imageSub_ = image_transport::create_subscription(node, "image", std::bind(&CameraROS::imgReceivedCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+	}
+	else
+	{
+		int queueSize = 10;
+		bool approxSync = true;
+		queueSize = node->declare_parameter("queue_size", queueSize);
+		approxSync = node->declare_parameter("approx_sync", approxSync);
+		RCLCPP_INFO(node->get_logger(), "find_object_ros: queue_size = %d", queueSize);
+		RCLCPP_INFO(node->get_logger(), "find_object_ros: approx_sync = %s", approxSync?"true":"false");
+
+
+		image_transport::TransportHints hints(node);
+		rgbSub_.subscribe(node, "rgb/image_rect_color", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+		depthSub_.subscribe(node, "depth_registered/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+		cameraInfoSub_.subscribe(node, "depth_registered/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
+			approxSync_->registerCallback(std::bind(&CameraROS::imgDepthReceivedCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
+			exactSync_->registerCallback(std::bind(&CameraROS::imgDepthReceivedCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+	}
+}
+CameraROS::~CameraROS()
+{
+	delete approxSync_;
+	delete exactSync_;
+}
+
+void CameraROS::setupExecutor(std::shared_ptr<rclcpp::Node> node)
+{
+	executor_.add_node(node);
+}
+
+QStringList CameraROS::subscribedTopics() const
+{
+	QStringList topics;
+	if(subscribeDepth_)
+	{
+		topics.append(rgbSub_.getTopic().c_str());
+		topics.append(depthSub_.getTopic().c_str());
+		topics.append(cameraInfoSub_.getSubscriber()->get_topic_name());
+	}
+	else
+	{
+		topics.append(imageSub_.getTopic().c_str());
+	}
+	return topics;
+}
+
+bool CameraROS::start()
+{
+	this->startTimer();
+	return true;
+}
+
+void CameraROS::stop()
+{
+	this->stopTimer();
+}
+
+void CameraROS::takeImage()
+{
+	executor_.spin_some();
+}
+
+void CameraROS::imgReceivedCallback(const sensor_msgs::msg::Image::ConstSharedPtr msg)
+{
+	if(msg->data.size())
+	{
+		cv::Mat image;
+		cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(msg);
+		try
+		{
+			if(msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			   msg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
+			}
+			else
+			{
+				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
+			}
+
+			Q_EMIT imageReceived(image, Header(msg->header.frame_id.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec), cv::Mat(), 0.0f);
+		}
+		catch(const cv_bridge::Exception & e)
+		{
+			RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", msg->encoding.c_str(), e.what());
+		}
+	}
+}
+
+void CameraROS::imgDepthReceivedCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
+{
+	if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
+	{
+			RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Depth image type must be 32FC1 or 16UC1");
+			return;
+	}
+
+	if(rgbMsg->data.size())
+	{
+		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+		float depthConstant = 1.0f/cameraInfoMsg->k[4];
+
+		cv::Mat image;
+		cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(rgbMsg);
+		try
+		{
+			if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			   rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
+			}
+			else
+			{
+				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
+			}
+
+			Q_EMIT imageReceived(image, Header(rgbMsg->header.frame_id.c_str(), rgbMsg->header.stamp.sec, rgbMsg->header.stamp.nanosec), ptrDepth->image, depthConstant);
+		}
+		catch(const cv_bridge::Exception & e)
+		{
+			RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", rgbMsg->encoding.c_str(), e.what());
+		}
+	}
+
+
+}

src/ros2/CameraROS.h

@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 CAMERAROS_H_
+#define CAMERAROS_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include "find_object/Camera.h"
+#include <QtCore/QStringList>
+
+class CameraROS : public find_object::Camera {
+	Q_OBJECT
+public:
+	CameraROS(bool subscribeDepth, rclcpp::Node * node);
+	virtual ~CameraROS();
+	void setupExecutor(std::shared_ptr<rclcpp::Node> node);
+
+	virtual bool start();
+	virtual void stop();
+
+	QStringList subscribedTopics() const;
+
+private Q_SLOTS:
+	virtual void takeImage();
+
+private:
+	void imgReceivedCallback(const sensor_msgs::msg::Image::ConstSharedPtr msg);
+	void imgDepthReceivedCallback(
+			const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
+			const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
+
+private:
+	rclcpp::Node * node_;
+	rclcpp::executors::SingleThreadedExecutor executor_;
+	bool subscribeDepth_;
+	image_transport::Subscriber imageSub_;
+
+	image_transport::SubscriberFilter rgbSub_;
+	image_transport::SubscriberFilter depthSub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::msg::Image,
+			sensor_msgs::msg::Image,
+			sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::msg::Image,
+			sensor_msgs::msg::Image,
+			sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+#endif /* CAMERAROS_H_ */

src/ros2/FindObjectROS.cpp

@@ -0,0 +1,366 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 "FindObjectROS.h"
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#ifdef PRE_ROS_HUMBLE
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <cmath>
+
+using namespace find_object;
+
+FindObjectROS::FindObjectROS(rclcpp::Node * node) :
+	FindObject(true),
+	node_(node),
+	objFramePrefix_("object"),
+	usePnP_(true)
+{
+	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+	objFramePrefix_ = node->declare_parameter("object_prefix", objFramePrefix_);
+	usePnP_ = node->declare_parameter("pnp", usePnP_);
+	RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str());
+	RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false");
+
+	pub_ = node->create_publisher<std_msgs::msg::Float32MultiArray>("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
+	pubStamped_ = node->create_publisher<find_object_2d::msg::ObjectsStamped>("objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
+	pubInfo_ = node->create_publisher<find_object_2d::msg::DetectionInfo>("info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
+
+	this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
+}
+
+void FindObjectROS::publish(const find_object::DetectionInfo & info, const Header & header, const cv::Mat & depth, float depthConstant)
+{
+	// send tf before the message
+	if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
+	{
+		std::vector<geometry_msgs::msg::TransformStamped> transforms;
+		char multiSubId = 'b';
+		int previousId = -1;
+		QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
+		for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
+			iter!=info.objDetected_.constEnd();
+			++iter, ++iterSizes)
+		{
+			// get data
+			int id = iter.key();
+			float objectWidth = iterSizes->width();
+			float objectHeight = iterSizes->height();
+
+			QString multiSuffix;
+			if(id == previousId)
+			{
+				multiSuffix = QString("_") + multiSubId++;
+			}
+			else
+			{
+				multiSubId = 'b';
+			}
+			previousId = id;
+
+			// Find center of the object
+			QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
+			cv::Vec3f center3D = this->getDepth(depth,
+					center.x()+0.5f, center.y()+0.5f,
+					float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+					1.0f/depthConstant, 1.0f/depthConstant);
+
+			cv::Vec3f axisEndX;
+			cv::Vec3f axisEndY;
+			if(!usePnP_)
+			{
+				QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
+				axisEndX = this->getDepth(depth,
+						xAxis.x()+0.5f, xAxis.y()+0.5f,
+						float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+						1.0f/depthConstant, 1.0f/depthConstant);
+
+				QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
+				axisEndY = this->getDepth(depth,
+						yAxis.x()+0.5f, yAxis.y()+0.5f,
+						float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
+						1.0f/depthConstant, 1.0f/depthConstant);
+			}
+
+			if((std::isfinite(center3D.val[2]) && usePnP_) ||
+				(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
+				 std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
+				 std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2])))
+			{
+				geometry_msgs::msg::TransformStamped transform;
+				transform.transform.rotation.x=0;
+				transform.transform.rotation.y=0;
+				transform.transform.rotation.z=0;
+				transform.transform.rotation.w=1;
+				transform.transform.translation.x=0;
+				transform.transform.translation.y=0;
+				transform.transform.translation.z=0;
+				transform.child_frame_id = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
+				transform.header.frame_id = header.frameId_.toStdString();
+				transform.header.stamp.sec = header.sec_;
+				transform.header.stamp.nanosec = header.nsec_;
+
+				tf2::Quaternion q;
+				if(usePnP_)
+				{
+					std::vector<cv::Point3f> objectPoints(4);
+					std::vector<cv::Point2f> imagePoints(4);
+
+					objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
+					objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
+					objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
+					objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
+
+					QPointF pt = iter->map(QPointF(0, 0));
+					imagePoints[0] = cv::Point2f(pt.x(), pt.y());
+					pt = iter->map(QPointF(objectWidth, 0));
+					imagePoints[1] = cv::Point2f(pt.x(), pt.y());
+					pt = iter->map(QPointF(objectWidth, objectHeight));
+					imagePoints[2] = cv::Point2f(pt.x(), pt.y());
+					pt = iter->map(QPointF(0, objectHeight));
+					imagePoints[3] = cv::Point2f(pt.x(), pt.y());
+
+					cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
+					cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
+					cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
+					cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
+					cameraMatrix.at<double>(1,2) = float(depth.rows/2)-0.5f;
+					cv::Mat distCoeffs;
+
+					cv::Mat rvec(1,3, CV_64FC1);
+					cv::Mat tvec(1,3, CV_64FC1);
+					cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
+
+					cv::Mat R;
+					cv::Rodrigues(rvec, R);
+					tf2::Matrix3x3 rotationMatrix(
+							R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
+							R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
+							R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
+					rotationMatrix.getRotation(q);
+					transform.transform.translation.x = tvec.at<double>(0)*(center3D.val[2]/tvec.at<double>(2));
+					transform.transform.translation.y = tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2));
+					transform.transform.translation.z = tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(2));
+				}
+				else
+				{
+					tf2::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
+					xAxis.normalize();
+					tf2::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
+					yAxis.normalize();
+					tf2::Vector3 zAxis = xAxis.cross(yAxis);
+					zAxis.normalize();
+					tf2::Matrix3x3 rotationMatrix(
+								xAxis.x(), yAxis.x() ,zAxis.x(),
+								xAxis.y(), yAxis.y(), zAxis.y(),
+								xAxis.z(), yAxis.z(), zAxis.z());
+					rotationMatrix.getRotation(q);
+					transform.transform.translation.x = center3D.val[0];
+					transform.transform.translation.y = center3D.val[1];
+					transform.transform.translation.z = center3D.val[2];
+				}
+
+				// set x axis going front of the object, with z up and y left
+				tf2::Quaternion q2;
+				q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0);
+				q *= q2;
+				transform.transform.rotation = tf2::toMsg(q.normalized());
+				transforms.push_back(transform);
+			}
+			else
+			{
+				RCLCPP_WARN(node_->get_logger(), "Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
+						 "(maybe object is too near of the camera or bad depth image)\n",
+						id,
+						center.x(), center.y(),
+						QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
+			}
+		}
+		if(transforms.size())
+		{
+			tfBroadcaster_->sendTransform(transforms);
+		}
+	}
+
+	if(pub_->get_subscription_count() || pubStamped_->get_subscription_count() || pubInfo_->get_subscription_count())
+	{
+		std_msgs::msg::Float32MultiArray msg;
+		find_object_2d::msg::ObjectsStamped msgStamped;
+		find_object_2d::msg::DetectionInfo infoMsg;
+		if(pubInfo_->get_subscription_count())
+		{
+			infoMsg.ids.resize(info.objDetected_.size());
+			infoMsg.widths.resize(info.objDetected_.size());
+			infoMsg.heights.resize(info.objDetected_.size());
+			infoMsg.file_paths.resize(info.objDetected_.size());
+			infoMsg.inliers.resize(info.objDetected_.size());
+			infoMsg.outliers.resize(info.objDetected_.size());
+			infoMsg.homographies.resize(info.objDetected_.size());
+		}
+		msg.data = std::vector<float>(info.objDetected_.size()*12);
+		msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
+
+		Q_ASSERT(info.objDetected_.size() == info.objDetectedSizes_.size() &&
+				   info.objDetected_.size() == info.objDetectedFilePaths_.size() &&
+				   info.objDetected_.size() == info.objDetectedInliersCount_.size() &&
+				   info.objDetected_.size() == info.objDetectedOutliersCount_.size());
+
+		int infoIndex=0;
+		int i=0;
+		QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
+		QMultiMap<int, QString>::const_iterator iterFilePaths=info.objDetectedFilePaths_.constBegin();
+		QMultiMap<int, int>::const_iterator iterInliers=info.objDetectedInliersCount_.constBegin();
+		QMultiMap<int, int>::const_iterator iterOutliers=info.objDetectedOutliersCount_.constBegin();
+		for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
+			iter!=info.objDetected_.constEnd();
+			++iter, ++iterSizes, ++iterFilePaths, ++infoIndex, ++iterInliers, ++iterOutliers)
+		{
+			if(pub_->get_subscription_count() || pubStamped_->get_subscription_count())
+			{
+				msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
+				msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
+				msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
+				msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
+			}
+
+			if(pubInfo_->get_subscription_count())
+			{
+				infoMsg.ids[infoIndex].data = iter.key();
+				infoMsg.widths[infoIndex].data = iterSizes->width();
+				infoMsg.heights[infoIndex].data = iterSizes->height();
+				infoMsg.file_paths[infoIndex].data = iterFilePaths.value().toStdString();
+				infoMsg.inliers[infoIndex].data = iterInliers.value();
+				infoMsg.outliers[infoIndex].data = iterOutliers.value();
+				infoMsg.homographies[infoIndex].data.resize(9);
+				infoMsg.homographies[infoIndex].data[0] = iter->m11();
+				infoMsg.homographies[infoIndex].data[1] = iter->m12();
+				infoMsg.homographies[infoIndex].data[2] = iter->m13();
+				infoMsg.homographies[infoIndex].data[3] = iter->m21();
+				infoMsg.homographies[infoIndex].data[4] = iter->m22();
+				infoMsg.homographies[infoIndex].data[5] = iter->m23();
+				infoMsg.homographies[infoIndex].data[6] = iter->m31();
+				infoMsg.homographies[infoIndex].data[7] = iter->m32();
+				infoMsg.homographies[infoIndex].data[8] = iter->m33();
+			}
+		}
+		if(pub_->get_subscription_count())
+		{
+			pub_->publish(msg);
+		}
+		if(pubStamped_->get_subscription_count())
+		{
+			// use same header as the input image (for synchronization and frame reference)
+			msgStamped.header.frame_id = header.frameId_.toStdString();
+			msgStamped.header.stamp.sec = header.sec_;
+			msgStamped.header.stamp.nanosec = header.nsec_;
+			pubStamped_->publish(msgStamped);
+		}
+		if(pubInfo_->get_subscription_count())
+		{
+			// use same header as the input image (for synchronization and frame reference)
+			infoMsg.header.frame_id = header.frameId_.toStdString();
+			infoMsg.header.stamp.sec = header.sec_;
+			infoMsg.header.stamp.nanosec = header.nsec_;
+			pubInfo_->publish(infoMsg);
+		}
+	}
+}
+
+cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
+				   int x, int y,
+				   float cx, float cy,
+				   float fx, float fy)
+{
+	if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
+	{
+		RCLCPP_ERROR(node_->get_logger(), "Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
+				x, y,
+				depthImage.cols, depthImage.rows);
+		return cv::Vec3f(
+				std::numeric_limits<float>::quiet_NaN (),
+				std::numeric_limits<float>::quiet_NaN (),
+				std::numeric_limits<float>::quiet_NaN ());
+	}
+
+
+	cv::Vec3f pt;
+
+	// Use correct principal point from calibration
+	float center_x = cx; //cameraInfo.K.at(2)
+	float center_y = cy; //cameraInfo.K.at(5)
+
+	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
+
+	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+	float unit_scaling = isInMM?0.001f:1.0f;
+	float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
+	float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	float depth;
+	bool isValid;
+	if(isInMM)
+	{
+		depth = (float)depthImage.at<uint16_t>(y,x);
+		isValid = depth != 0.0f;
+	}
+	else
+	{
+		depth = depthImage.at<float>(y,x);
+		isValid = std::isfinite(depth) && depth > 0.0f;
+	}
+
+	// Check for invalid measurements
+	if (!isValid)
+	{
+		pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
+	}
+	else
+	{
+		// Fill in XYZ
+		pt.val[0] = (float(x) - center_x) * depth * constant_x;
+		pt.val[1] = (float(y) - center_y) * depth * constant_y;
+		pt.val[2] = depth*unit_scaling;
+	}
+	return pt;
+}

src/ros2/FindObjectROS.h

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 FINDOBJECTROS_H_
+#define FINDOBJECTROS_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include "find_object/FindObject.h"
+
+#include <std_msgs/msg/float32_multi_array.hpp>
+#include "find_object_2d/msg/objects_stamped.hpp"
+#include "find_object_2d/msg/detection_info.hpp"
+
+#include <QtCore/QObject>
+#include <QtCore/QMultiMap>
+#include <QtCore/QPair>
+#include <QtCore/QRect>
+#include <QtGui/QTransform>
+
+class FindObjectROS : public find_object::FindObject
+{
+	Q_OBJECT;
+
+public:
+	FindObjectROS(rclcpp::Node * node);
+	virtual ~FindObjectROS() {}
+
+public Q_SLOTS:
+	void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
+
+private:
+	cv::Vec3f getDepth(const cv::Mat & depthImage,
+					   int x, int y,
+					   float cx, float cy,
+					   float fx, float fy);
+
+private:
+	rclcpp::Node * node_;
+	rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr pub_;
+	rclcpp::Publisher<find_object_2d::msg::ObjectsStamped>::SharedPtr pubStamped_;
+	rclcpp::Publisher<find_object_2d::msg::DetectionInfo>::SharedPtr pubInfo_;
+
+	std::string objFramePrefix_;
+	bool usePnP_;
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+
+};
+
+#endif /* FINDOBJECTROS_H_ */

src/ros2/find_object_2d_node.cpp

@@ -0,0 +1,217 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 "CameraROS.h"
+#include "FindObjectROS.h"
+
+#include <QApplication>
+#include <QDir>
+#include "find_object/MainWindow.h"
+#include "ParametersToolBox.h"
+#include "find_object/Settings.h"
+#include <signal.h>
+
+void my_handler_gui(int s){
+	QApplication::closeAllWindows();
+	QApplication::quit();
+}
+void my_handler(int s){
+	QCoreApplication::quit();
+}
+
+void setupQuitSignal(bool gui)
+{
+	// Catch ctrl-c to close the gui
+	struct sigaction sigIntHandler;
+	if(gui)
+	{
+		sigIntHandler.sa_handler = my_handler_gui;
+	}
+	else
+	{
+		sigIntHandler.sa_handler = my_handler;
+	}
+	sigemptyset(&sigIntHandler.sa_mask);
+	sigIntHandler.sa_flags = 0;
+	sigaction(SIGINT, &sigIntHandler, NULL);
+}
+
+class FindObjectNode : public rclcpp::Node {
+
+public:
+	FindObjectNode() :
+		rclcpp::Node("find_object_2d"),
+		findObjectROS_(0),
+		camera_(0),
+		gui_(true)
+	{
+		std::string objectsPath;
+		std::string sessionPath;
+		std::string settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
+		bool subscribeDepth = false;
+
+		gui_ = this->declare_parameter("gui", gui_);
+		objectsPath = this->declare_parameter("objects_path", objectsPath);
+		sessionPath = this->declare_parameter("session_path", sessionPath);
+		settingsPath = this->declare_parameter("settings_path", settingsPath);
+		subscribeDepth = this->declare_parameter("subscribe_depth", subscribeDepth);
+
+		RCLCPP_INFO(this->get_logger(), "gui=%d", gui_?1:0);
+		RCLCPP_INFO(this->get_logger(), "objects_path=%s", objectsPath.c_str());
+		RCLCPP_INFO(this->get_logger(), "session_path=%s", sessionPath.c_str());
+		RCLCPP_INFO(this->get_logger(), "settings_path=%s", settingsPath.c_str());
+		RCLCPP_INFO(this->get_logger(), "subscribe_depth = %s", subscribeDepth?"true":"false");
+
+		if(settingsPath.empty())
+		{
+			settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
+		}
+		else
+		{
+			if(!sessionPath.empty())
+			{
+				RCLCPP_WARN(this->get_logger(), "\"settings_path\" parameter is ignored when \"session_path\" is set.");
+			}
+
+			QString path = settingsPath.c_str();
+			if(path.contains('~'))
+			{
+				path.replace('~', QDir::homePath());
+				settingsPath = path.toStdString();
+			}
+		}
+
+		// Load settings, should be loaded before creating other objects
+		find_object::Settings::init(settingsPath.c_str());
+
+		findObjectROS_ = new FindObjectROS(this);
+		if(!sessionPath.empty())
+		{
+			if(!objectsPath.empty())
+			{
+				RCLCPP_WARN(this->get_logger(), "\"objects_path\" parameter is ignored when \"session_path\" is set.");
+			}
+			if(!findObjectROS_->loadSession(sessionPath.c_str()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to load session \"%s\"", sessionPath.c_str());
+			}
+		}
+		else if(!objectsPath.empty())
+		{
+			QString path = objectsPath.c_str();
+			if(path.contains('~'))
+			{
+				path.replace('~', QDir::homePath());
+			}
+			if(!findObjectROS_->loadObjects(path))
+			{
+				RCLCPP_ERROR(this->get_logger(), "No objects loaded from path \"%s\"", path.toStdString().c_str());
+			}
+		}
+		camera_ = new CameraROS(subscribeDepth, this);
+
+		// Catch ctrl-c to close the gui
+		setupQuitSignal(gui_);
+	}
+
+	void exec(int argc, char ** argv, std::shared_ptr<rclcpp::Node> node)
+	{
+		camera_->setupExecutor(node);
+		if(gui_)
+		{
+			QApplication app(argc, argv);
+			find_object::MainWindow mainWindow(findObjectROS_, camera_); // take ownership
+
+			QObject::connect(
+					&mainWindow,
+					SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)),
+					findObjectROS_,
+					SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
+
+			QStringList topics = camera_->subscribedTopics();
+			if(topics.size() == 1)
+			{
+				mainWindow.setSourceImageText(mainWindow.tr(
+						"<qt>Find-Object subscribed to <b>%1</b> topic.<br/>"
+						"You can remap the topic when starting the node: <br/>\"rosrun find_object_2d find_object_2d image:=your/image/topic\".<br/>"
+						"</qt>").arg(topics.first()));
+			}
+			else if(topics.size() == 3)
+			{
+				mainWindow.setSourceImageText(mainWindow.tr(
+						"<qt>Find-Object subscribed to : <br/> <b>%1</b> <br/> <b>%2</b> <br/> <b>%3</b><br/>"
+						"</qt>").arg(topics.at(0)).arg(topics.at(1)).arg(topics.at(2)));
+			}
+			mainWindow.show();
+			app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) );
+
+			// mainWindow has ownership
+			findObjectROS_ = 0;
+			camera_ = 0;
+
+			// loop
+			mainWindow.startProcessing();
+			app.exec();
+			find_object::Settings::saveSettings();
+		}
+		else
+		{
+			QCoreApplication app(argc, argv);
+
+			// connect stuff:
+			QObject::connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), findObjectROS_, SLOT(detect(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
+
+			//loop
+			camera_->start();
+			app.exec();
+
+			delete camera_;
+			camera_=0;
+			delete findObjectROS_;
+			findObjectROS_=0;
+		}
+	}
+
+	virtual ~FindObjectNode()
+	{
+		delete findObjectROS_;
+		delete camera_;
+	}
+
+private:
+	FindObjectROS * findObjectROS_;
+	CameraROS * camera_;
+	bool gui_;
+};
+
+int main(int argc, char** argv)
+{
+	rclcpp::init(argc, argv);
+	auto node = std::make_shared<FindObjectNode>();
+	node->exec(argc, argv, node);
+	rclcpp::shutdown();
+}

src/ros2/print_objects_detected_node.cpp

@@ -0,0 +1,183 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 <rclcpp/rclcpp.hpp>
+#include <find_object_2d/msg/objects_stamped.hpp>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <QTransform>
+#include <QColor>
+
+class PrintObjects: public rclcpp::Node
+{
+public:
+	PrintObjects() :
+		Node("objects_detected")
+	{
+		image_transport::TransportHints hints(this);
+
+		imagePub_ = image_transport::create_publisher(this, "image_with_objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+
+		// Simple subscriber
+		sub_ = create_subscription<std_msgs::msg::Float32MultiArray>("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1), std::bind(&PrintObjects::objectsDetectedCallback, this, std::placeholders::_1));
+
+		// Synchronized image + objects example
+		imageSub_.subscribe(this, "image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+		objectsSub_.subscribe(this, "objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
+
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(10), imageSub_, objectsSub_);
+		exactSync_->registerCallback(std::bind(&PrintObjects::imageObjectsDetectedCallback, this, std::placeholders::_1, std::placeholders::_2));
+	}
+
+	virtual ~PrintObjects()
+	{
+		delete exactSync_;
+	}
+
+	/**
+	 * IMPORTANT :
+	 *      Parameter General/MirrorView must be false
+	 *      Parameter Homography/homographyComputed must be true
+	 */
+	void objectsDetectedCallback(
+			const std_msgs::msg::Float32MultiArray::ConstSharedPtr msg)
+	{
+		printf("---\n");
+		const std::vector<float> & data = msg->data;
+		if(data.size())
+		{
+			for(unsigned int i=0; i<data.size(); i+=12)
+			{
+				// get data
+				int id = (int)data[i];
+				float objectWidth = data[i+1];
+				float objectHeight = data[i+2];
+
+				// Find corners Qt
+				QTransform qtHomography(data[i+3], data[i+4], data[i+5],
+										data[i+6], data[i+7], data[i+8],
+										data[i+9], data[i+10], data[i+11]);
+
+				QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
+				QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
+				QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
+				QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
+
+				printf("Object %d detected, Qt corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
+						id,
+						qtTopLeft.x(), qtTopLeft.y(),
+						qtTopRight.x(), qtTopRight.y(),
+						qtBottomLeft.x(), qtBottomLeft.y(),
+						qtBottomRight.x(), qtBottomRight.y());
+			}
+		}
+		else
+		{
+			printf("No objects detected.\n");
+		}
+	}
+	void imageObjectsDetectedCallback(
+			const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+			const find_object_2d::msg::ObjectsStamped::ConstSharedPtr objectsMsg)
+	{
+		if(imagePub_.getNumSubscribers() > 0)
+		{
+			const std::vector<float> & data = objectsMsg->objects.data;
+			if(data.size())
+			{
+				for(unsigned int i=0; i<data.size(); i+=12)
+				{
+					// get data
+					int id = (int)data[i];
+					float objectWidth = data[i+1];
+					float objectHeight = data[i+2];
+
+					// Find corners OpenCV
+					cv::Mat cvHomography(3, 3, CV_32F);
+					cvHomography.at<float>(0,0) = data[i+3];
+					cvHomography.at<float>(1,0) = data[i+4];
+					cvHomography.at<float>(2,0) = data[i+5];
+					cvHomography.at<float>(0,1) = data[i+6];
+					cvHomography.at<float>(1,1) = data[i+7];
+					cvHomography.at<float>(2,1) = data[i+8];
+					cvHomography.at<float>(0,2) = data[i+9];
+					cvHomography.at<float>(1,2) = data[i+10];
+					cvHomography.at<float>(2,2) = data[i+11];
+					std::vector<cv::Point2f> inPts, outPts;
+					inPts.push_back(cv::Point2f(0,0));
+					inPts.push_back(cv::Point2f(objectWidth,0));
+					inPts.push_back(cv::Point2f(objectWidth,objectHeight));
+					inPts.push_back(cv::Point2f(0,objectHeight));
+					inPts.push_back(cv::Point2f(objectWidth/2,objectHeight/2));
+					cv::perspectiveTransform(inPts, outPts, cvHomography);
+
+					cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageMsg);
+
+					cv_bridge::CvImage img;
+					img = *imageDepthPtr;
+					std::vector<cv::Point2i> outPtsInt;
+					outPtsInt.push_back(outPts[0]);
+					outPtsInt.push_back(outPts[1]);
+					outPtsInt.push_back(outPts[2]);
+					outPtsInt.push_back(outPts[3]);
+					QColor color(QColor((Qt::GlobalColor)((id % 10 + 7)==Qt::yellow?Qt::darkYellow:(id % 10 + 7))));
+					cv::Scalar cvColor(color.red(), color.green(), color.blue());
+					cv::polylines(img.image, outPtsInt, true, cvColor, 3);
+					cv::Point2i center = outPts[4];
+					cv::putText(img.image, QString("(%1, %2)").arg(center.x).arg(center.y).toStdString(), center, cv::FONT_HERSHEY_SIMPLEX, 0.6, cvColor, 2);
+					cv::circle(img.image, center, 1, cvColor, 3);
+					imagePub_.publish(img.toImageMsg());
+				}
+			}
+		}
+	}
+
+private:
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::msg::Image,
+			find_object_2d::msg::ObjectsStamped> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	image_transport::Publisher imagePub_;
+	image_transport::SubscriberFilter imageSub_;
+	message_filters::Subscriber<find_object_2d::msg::ObjectsStamped> objectsSub_;
+	rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_;
+
+};
+
+
+int main(int argc, char** argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<PrintObjects>());
+	rclcpp::shutdown();
+
+    return 0;
+}

src/ros2/tf_example_node.cpp

@@ -0,0 +1,122 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 HOLDER 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 <rclcpp/rclcpp.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <find_object_2d/msg/objects_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <QtCore/QString>
+
+class TfExample : public rclcpp::Node
+{
+public:
+	TfExample() :
+		Node("tf_example_node"),
+		objFramePrefix_("object")
+	{
+		tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+		//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+		//	this->get_node_base_interface(),
+		//	this->get_node_timers_interface());
+		//tfBuffer_->setCreateTimerInterface(timer_interface);
+		tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
+
+		targetFrameId_ = this->declare_parameter("target_frame_id", targetFrameId_);
+		objFramePrefix_ = this->declare_parameter("object_prefix", objFramePrefix_);
+
+		subs_ = create_subscription<find_object_2d::msg::ObjectsStamped>("objectsStamped", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)1), std::bind(&TfExample::objectsDetectedCallback, this, std::placeholders::_1));
+	}
+
+	// Here I synchronize with the ObjectsStamped topic to
+	// know when the TF is ready and for which objects
+	void objectsDetectedCallback(const find_object_2d::msg::ObjectsStamped::ConstSharedPtr msg)
+	{
+		if(msg->objects.data.size())
+		{
+			std::string targetFrameId = targetFrameId_;
+			if(targetFrameId.empty())
+			{
+				targetFrameId = msg->header.frame_id;
+			}
+			char multiSubId = 'b';
+			int previousId = -1;
+			for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
+			{
+				// get data
+				int id = (int)msg->objects.data[i];
+
+				QString multiSuffix;
+				if(id == previousId)
+				{
+					multiSuffix = QString("_") + multiSubId++;
+				}
+				else
+				{
+					multiSubId = 'b';
+				}
+				previousId = id;
+
+				// "object_1", "object_1_b", "object_1_c", "object_2"
+				std::string objectFrameId = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
+
+				geometry_msgs::msg::TransformStamped pose;
+				try
+				{
+					// Get transformation from "object_#" frame to target frame
+					// The timestamp matches the one sent over TF
+					pose = tfBuffer_->lookupTransform(targetFrameId, objectFrameId, tf2_ros::fromMsg(msg->header.stamp));
+				}
+				catch(tf2::TransformException & ex)
+				{
+					RCLCPP_WARN(this->get_logger(), "%s",ex.what());
+					continue;
+				}
+
+				// Here "pose" is the position of the object "id" in target frame.
+				RCLCPP_INFO(this->get_logger(), "%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
+						objectFrameId.c_str(), targetFrameId.c_str(),
+						pose.transform.translation.x, pose.transform.translation.y, pose.transform.translation.z,
+						pose.transform.rotation.x, pose.transform.rotation.y, pose.transform.rotation.z, pose.transform.rotation.w);
+			}
+		}
+	}
+
+private:
+	std::string targetFrameId_;
+	std::string objFramePrefix_;
+	rclcpp::Subscription<find_object_2d::msg::ObjectsStamped>::SharedPtr subs_;
+    std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+};
+
+int main(int argc, char * argv[])
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<TfExample>());
+	rclcpp::shutdown();
+}