diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml
new file mode 100644
index 0000000..273581a
--- /dev/null
+++ b/.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}}
+
+
diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml
new file mode 100644
index 0000000..bcc9713
--- /dev/null
+++ b/.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
diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml
new file mode 100644
index 0000000..364bebb
--- /dev/null
+++ b/.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+
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b8923e7..f2e52ff 100644
--- a/CMakeLists.txt
+++ b/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
- launch/find_object_3d_kinect2.launch
- launch/find_object_3d_zed.launch
- launch/find_object_2d.launch
- launch/find_object_3d.launch
+ install(DIRECTORY
+ launch/ros1
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()
diff --git a/README.md b/README.md
index 4a3831c..7dcfbd4 100644
--- a/README.md
+++ b/README.md
@@ -1,48 +1,92 @@
-## find-object (standalone)
-Linux: [](https://travis-ci.com/introlab/find-object) Windows: [](https://ci.appveyor.com/project/matlabbe/find-object/branch/master)
+# find-object
+
+
+
+ | Linux |
+ 
+ |
+
+
+ | Windows |
+ 
+ |
+
+
+
+
+## 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-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
+sudo apt-get install ros-$ROS_DISTRO-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).
+
+```bash
+cd ~/catkin_ws
+git clone https://github.com/introlab/find-object.git src/find_object_2d
+catkin_make
+```
+
+### Run
+```bash
+roscore
+# Launch your preferred usb camera driver
+rosrun uvc_camera uvc_camera_node
+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
- * 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.
+### Install
+
+Binaries:
+```bash
+To come...
+```
- * 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.
+Source:
```bash
-# Install ROS Groovy/Hydro/Indigo/Jade/Kinetic (catkin build):
- $ cd ~/catkin_ws
- $ git clone https://github.com/introlab/find-object.git src/find_object_2d
- $ 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
+cd ~/ros2_ws
+git clone https://github.com/introlab/find-object.git src/find_object_2d
+colcon build
```
### Run
```bash
- $ roscore &
- # Launch your preferred usb camera driver
- $ rosrun uvc_camera uvc_camera_node &
- $ rosrun find_object_2d find_object_2d image:=image_raw
+# 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
```
-See [find_object_2d](http://wiki.ros.org/find_object_2d) for more information.
+#### 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
+```
+
diff --git a/launch/find_object_2d.launch b/launch/ros1/find_object_2d.launch
similarity index 100%
rename from launch/find_object_2d.launch
rename to launch/ros1/find_object_2d.launch
diff --git a/launch/find_object_3d.launch b/launch/ros1/find_object_3d.launch
similarity index 100%
rename from launch/find_object_3d.launch
rename to launch/ros1/find_object_3d.launch
diff --git a/launch/find_object_3d_kinect2.launch b/launch/ros1/find_object_3d_kinect2.launch
similarity index 100%
rename from launch/find_object_3d_kinect2.launch
rename to launch/ros1/find_object_3d_kinect2.launch
diff --git a/launch/find_object_3d_zed.launch b/launch/ros1/find_object_3d_zed.launch
similarity index 100%
rename from launch/find_object_3d_zed.launch
rename to launch/ros1/find_object_3d_zed.launch
diff --git a/launch/ros2/find_object_2d.launch.py b/launch/ros2/find_object_2d.launch.py
new file mode 100644
index 0000000..504242f
--- /dev/null
+++ b/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'))]),
+ ])
diff --git a/launch/ros2/find_object_3d.launch.py b/launch/ros2/find_object_3d.launch.py
new file mode 100644
index 0000000..e883486
--- /dev/null
+++ b/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'))]),
+ ])
diff --git a/msg/DetectionInfo.msg b/msg/DetectionInfo.msg
index bba88ae..d9825f0 100644
--- a/msg/DetectionInfo.msg
+++ b/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)
diff --git a/msg/ObjectsStamped.msg b/msg/ObjectsStamped.msg
index fc34a83..c1d191a 100644
--- a/msg/ObjectsStamped.msg
+++ b/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
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 3977342..d519aae 100644
--- a/package.xml
+++ b/package.xml
@@ -1,38 +1,52 @@
-
+
+
find_object_2d
0.6.4
The find_object_2d package
-
Mathieu Labbe
+ Mathieu Labbe
BSD
- http://find-object.googlecode.com
- Mathieu Labbe
+ https://github.com/introlab/find-object/issues
+ https://github.com/introlab/find-object
- catkin
+ catkin
+ ament_cmake
+
+ roscpp
+ tf
+ rclcpp
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+
+ ros_environment
+ builtin_interfaces
+ rosidl_default_generators
+ rosidl_default_runtime
+ rosidl_interface_packages
- message_generation
+ message_generation
qtbase5-dev
cv_bridge
- roscpp
- rospy
sensor_msgs
std_msgs
std_srvs
+ geometry_msgs
image_transport
message_filters
- tf
- message_runtime
- qtbase5-dev
- cv_bridge
- roscpp
- rospy
- sensor_msgs
- std_msgs
- std_srvs
- image_transport
- message_filters
- tf
+ message_runtime
+ qtbase5-dev
+ cv_bridge
+ sensor_msgs
+ std_msgs
+ std_srvs
+ geometry_msgs
+ image_transport
+ message_filters
+
+ ament_cmake
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d38f0c7..c6a95a6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -19,10 +19,17 @@ SET(headers_ui
IF(CATKIN_BUILD)
SET(headers_ui
${headers_ui}
- ./ros/CameraROS.h
- ./ros/FindObjectROS.h
+ ./ros1/CameraROS.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
- ./ros/FindObjectROS.cpp
+ ./ros1/CameraROS.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("$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")
-IF(NOT CATKIN_BUILD)
+ 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()
- add_executable(find_object_2d ros/find_object_2d_node.cpp)
+ELSEIF(CATKIN_BUILD)
+ 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()
diff --git a/src/ros/CameraROS.cpp b/src/ros1/CameraROS.cpp
similarity index 100%
rename from src/ros/CameraROS.cpp
rename to src/ros1/CameraROS.cpp
index 9082763..da1ca00 100644
--- a/src/ros/CameraROS.cpp
+++ b/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
+#include "CameraROS.h"
#include
using namespace find_object;
diff --git a/src/ros/CameraROS.h b/src/ros1/CameraROS.h
similarity index 100%
rename from src/ros/CameraROS.h
rename to src/ros1/CameraROS.h
diff --git a/src/ros/FindObjectROS.cpp b/src/ros1/FindObjectROS.cpp
similarity index 99%
rename from src/ros/FindObjectROS.cpp
rename to src/ros1/FindObjectROS.cpp
index 8650ab5..d814534 100644
--- a/src/ros/FindObjectROS.cpp
+++ b/src/ros1/FindObjectROS.cpp
@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "FindObjectROS.h"
-
#include
#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);
diff --git a/src/ros/FindObjectROS.h b/src/ros1/FindObjectROS.h
similarity index 100%
rename from src/ros/FindObjectROS.h
rename to src/ros1/FindObjectROS.h
diff --git a/src/ros/find_object_2d_node.cpp b/src/ros1/find_object_2d_node.cpp
similarity index 98%
rename from src/ros/find_object_2d_node.cpp
rename to src/ros1/find_object_2d_node.cpp
index e54fb35..4d78206 100644
--- a/src/ros/find_object_2d_node.cpp
+++ b/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 "FindObjectROS.h"
-
+#include
+#include
#include
#include
#include "find_object/MainWindow.h"
diff --git a/src/ros/print_objects_detected_node.cpp b/src/ros1/print_objects_detected_node.cpp
similarity index 100%
rename from src/ros/print_objects_detected_node.cpp
rename to src/ros1/print_objects_detected_node.cpp
diff --git a/src/ros/tf_example_node.cpp b/src/ros1/tf_example_node.cpp
similarity index 100%
rename from src/ros/tf_example_node.cpp
rename to src/ros1/tf_example_node.cpp
diff --git a/src/ros2/CameraROS.cpp b/src/ros2/CameraROS.cpp
new file mode 100644
index 0000000..dc76ea9
--- /dev/null
+++ b/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
+#include
+
+using namespace find_object;
+
+CameraROS::CameraROS(bool subscribeDepth, rclcpp::Node * node) :
+ node_(node),
+ subscribeDepth_(subscribeDepth),
+ approxSync_(0),
+ exactSync_(0)
+{
+ qRegisterMetaType("ros::Time");
+ qRegisterMetaType("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(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(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 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());
+ }
+ }
+
+
+}
diff --git a/src/ros2/CameraROS.h b/src/ros2/CameraROS.h
new file mode 100644
index 0000000..c9e0f2c
--- /dev/null
+++ b/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
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include "find_object/Camera.h"
+#include
+
+class CameraROS : public find_object::Camera {
+ Q_OBJECT
+public:
+ CameraROS(bool subscribeDepth, rclcpp::Node * node);
+ virtual ~CameraROS();
+ void setupExecutor(std::shared_ptr 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 cameraInfoSub_;
+
+ typedef message_filters::sync_policies::ApproximateTime<
+ sensor_msgs::msg::Image,
+ sensor_msgs::msg::Image,
+ sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
+ message_filters::Synchronizer * approxSync_;
+
+ typedef message_filters::sync_policies::ExactTime<
+ sensor_msgs::msg::Image,
+ sensor_msgs::msg::Image,
+ sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
+ message_filters::Synchronizer * exactSync_;
+};
+
+#endif /* CAMERAROS_H_ */
diff --git a/src/ros2/FindObjectROS.cpp b/src/ros2/FindObjectROS.cpp
new file mode 100644
index 0000000..cc6d2a5
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#ifdef PRE_ROS_HUMBLE
+#include
+#else
+#include
+#endif
+
+#include
+
+using namespace find_object;
+
+FindObjectROS::FindObjectROS(rclcpp::Node * node) :
+ FindObject(true),
+ node_(node),
+ objFramePrefix_("object"),
+ usePnP_(true)
+{
+ tfBroadcaster_ = std::make_shared(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("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
+ pubStamped_ = node->create_publisher("objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
+ pubInfo_ = node->create_publisher("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 transforms;
+ char multiSubId = 'b';
+ int previousId = -1;
+ QMultiMap::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
+ for(QMultiMap::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 objectPoints(4);
+ std::vector 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(0,0) = 1.0f/depthConstant;
+ cameraMatrix.at(1,1) = 1.0f/depthConstant;
+ cameraMatrix.at(0,2) = float(depth.cols/2)-0.5f;
+ cameraMatrix.at(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(0,0), R.at(0,1), R.at(0,2),
+ R.at(1,0), R.at(1,1), R.at(1,2),
+ R.at(2,0), R.at(2,1), R.at(2,2));
+ rotationMatrix.getRotation(q);
+ transform.transform.translation.x = tvec.at(0)*(center3D.val[2]/tvec.at(2));
+ transform.transform.translation.y = tvec.at(1)*(center3D.val[2]/tvec.at(2));
+ transform.transform.translation.z = tvec.at(2)*(center3D.val[2]/tvec.at(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(info.objDetected_.size()*12);
+ msgStamped.objects.data = std::vector(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::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
+ QMultiMap::const_iterator iterFilePaths=info.objDetectedFilePaths_.constBegin();
+ QMultiMap::const_iterator iterInliers=info.objDetectedInliersCount_.constBegin();
+ QMultiMap::const_iterator iterOutliers=info.objDetectedOutliersCount_.constBegin();
+ for(QMultiMap::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=0 && yget_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::quiet_NaN (),
+ std::numeric_limits::quiet_NaN (),
+ std::numeric_limits::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::quiet_NaN ();
+
+ float depth;
+ bool isValid;
+ if(isInMM)
+ {
+ depth = (float)depthImage.at(y,x);
+ isValid = depth != 0.0f;
+ }
+ else
+ {
+ depth = depthImage.at(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;
+}
diff --git a/src/ros2/FindObjectROS.h b/src/ros2/FindObjectROS.h
new file mode 100644
index 0000000..73c3fc8
--- /dev/null
+++ b/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
+#include
+#include
+#include "find_object/FindObject.h"
+
+#include
+#include "find_object_2d/msg/objects_stamped.hpp"
+#include "find_object_2d/msg/detection_info.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+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::SharedPtr pub_;
+ rclcpp::Publisher::SharedPtr pubStamped_;
+ rclcpp::Publisher::SharedPtr pubInfo_;
+
+ std::string objFramePrefix_;
+ bool usePnP_;
+ std::shared_ptr tfBroadcaster_;
+
+};
+
+#endif /* FINDOBJECTROS_H_ */
diff --git a/src/ros2/find_object_2d_node.cpp b/src/ros2/find_object_2d_node.cpp
new file mode 100644
index 0000000..2cecc54
--- /dev/null
+++ b/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
+#include
+#include "find_object/MainWindow.h"
+#include "ParametersToolBox.h"
+#include "find_object/Settings.h"
+#include
+
+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 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(
+ "Find-Object subscribed to %1 topic.
"
+ "You can remap the topic when starting the node:
\"rosrun find_object_2d find_object_2d image:=your/image/topic\".
"
+ "").arg(topics.first()));
+ }
+ else if(topics.size() == 3)
+ {
+ mainWindow.setSourceImageText(mainWindow.tr(
+ "Find-Object subscribed to :
%1
%2
%3
"
+ "").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();
+ node->exec(argc, argv, node);
+ rclcpp::shutdown();
+}
diff --git a/src/ros2/print_objects_detected_node.cpp b/src/ros2/print_objects_detected_node.cpp
new file mode 100644
index 0000000..7d9cafb
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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("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(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 & data = msg->data;
+ if(data.size())
+ {
+ for(unsigned int i=0; i 0)
+ {
+ const std::vector & data = objectsMsg->objects.data;
+ if(data.size())
+ {
+ for(unsigned int i=0; i(0,0) = data[i+3];
+ cvHomography.at(1,0) = data[i+4];
+ cvHomography.at(2,0) = data[i+5];
+ cvHomography.at(0,1) = data[i+6];
+ cvHomography.at(1,1) = data[i+7];
+ cvHomography.at(2,1) = data[i+8];
+ cvHomography.at(0,2) = data[i+9];
+ cvHomography.at(1,2) = data[i+10];
+ cvHomography.at(2,2) = data[i+11];
+ std::vector 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 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 * exactSync_;
+ image_transport::Publisher imagePub_;
+ image_transport::SubscriberFilter imageSub_;
+ message_filters::Subscriber objectsSub_;
+ rclcpp::Subscription::SharedPtr sub_;
+
+};
+
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+
+ return 0;
+}
diff --git a/src/ros2/tf_example_node.cpp b/src/ros2/tf_example_node.cpp
new file mode 100644
index 0000000..7def21d
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+
+class TfExample : public rclcpp::Node
+{
+public:
+ TfExample() :
+ Node("tf_example_node"),
+ objFramePrefix_("object")
+ {
+ tfBuffer_ = std::make_shared(this->get_clock());
+ //auto timer_interface = std::make_shared(
+ // this->get_node_base_interface(),
+ // this->get_node_timers_interface());
+ //tfBuffer_->setCreateTimerInterface(timer_interface);
+ tfListener_ = std::make_shared(*tfBuffer_);
+
+ targetFrameId_ = this->declare_parameter("target_frame_id", targetFrameId_);
+ objFramePrefix_ = this->declare_parameter("object_prefix", objFramePrefix_);
+
+ subs_ = create_subscription("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; iobjects.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::SharedPtr subs_;
+ std::shared_ptr tfBuffer_;
+ std::shared_ptr tfListener_;
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+}