From 634224960d889d9982ed39ed0da9793c956daa8c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Dec 2022 23:39:35 -0800 Subject: [PATCH 01/11] Working ros2 version --- CMakeLists.txt | 72 +++- README.md | 48 ++- launch/{ => ros1}/find_object_2d.launch | 0 launch/{ => ros1}/find_object_3d.launch | 0 .../{ => ros1}/find_object_3d_kinect2.launch | 0 launch/{ => ros1}/find_object_3d_zed.launch | 0 launch/ros2/find_object_2d.launch.py | 30 ++ launch/ros2/find_object_3d.launch.py | 42 ++ msg/DetectionInfo.msg | 4 +- msg/ObjectsStamped.msg | 2 +- package.xml | 53 ++- src/CMakeLists.txt | 116 +++++- src/{ros => ros1}/CameraROS.cpp | 2 +- src/{ros => ros1}/CameraROS.h | 0 src/{ros => ros1}/FindObjectROS.cpp | 1 - src/{ros => ros1}/FindObjectROS.h | 0 src/{ros => ros1}/find_object_2d_node.cpp | 5 +- .../print_objects_detected_node.cpp | 0 src/{ros => ros1}/tf_example_node.cpp | 0 src/ros2/CameraROS.cpp | 188 +++++++++ src/ros2/CameraROS.h | 93 +++++ src/ros2/FindObjectROS.cpp | 362 ++++++++++++++++++ src/ros2/FindObjectROS.h | 75 ++++ src/ros2/find_object_2d_node.cpp | 217 +++++++++++ src/ros2/print_objects_detected_node.cpp | 183 +++++++++ src/ros2/tf_example_node.cpp | 122 ++++++ 26 files changed, 1562 insertions(+), 53 deletions(-) rename launch/{ => ros1}/find_object_2d.launch (100%) rename launch/{ => ros1}/find_object_3d.launch (100%) rename launch/{ => ros1}/find_object_3d_kinect2.launch (100%) rename launch/{ => ros1}/find_object_3d_zed.launch (100%) create mode 100644 launch/ros2/find_object_2d.launch.py create mode 100644 launch/ros2/find_object_3d.launch.py rename src/{ros => ros1}/CameraROS.cpp (100%) rename src/{ros => ros1}/CameraROS.h (100%) rename src/{ros => ros1}/FindObjectROS.cpp (99%) rename src/{ros => ros1}/FindObjectROS.h (100%) rename src/{ros => ros1}/find_object_2d_node.cpp (98%) rename src/{ros => ros1}/print_objects_detected_node.cpp (100%) rename src/{ros => ros1}/tf_example_node.cpp (100%) create mode 100644 src/ros2/CameraROS.cpp create mode 100644 src/ros2/CameraROS.h create mode 100644 src/ros2/FindObjectROS.cpp create mode 100644 src/ros2/FindObjectROS.h create mode 100644 src/ros2/find_object_2d_node.cpp create mode 100644 src/ros2/print_objects_detected_node.cpp create mode 100644 src/ros2/tf_example_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b8923e7..0317f9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,18 +1,30 @@ -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 CATKIN_BUILD OR COLCON_BUILD) -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 +142,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,7 +370,7 @@ IF(NOT CATKIN_BUILD) ENDIF(APPLE) MESSAGE(STATUS "--------------------------------------------") -ELSE() +ELSEIF(CATKIN_BUILD) #ROS Catkin build ## Find catkin macros and libraries @@ -405,12 +417,48 @@ 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(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..90079e1 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ Linux: [![Build Status](https://travis-ci.com/introlab/find-object.svg?branch=ma Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. -## find_object_2d (ROS package) +## find_object_2d (ROS1 package) ### Install @@ -46,3 +46,49 @@ Source: $ rosrun find_object_2d find_object_2d image:=image_raw ``` See [find_object_2d](http://wiki.ros.org/find_object_2d) for more information. + +## find_object_2d (ROS2 package) + +### Install + +Binaries: +```bash +To come... +``` + +Source: + +```bash +# Install ROS Foxy/Galactic/Humble (colcon build): + $ 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 + [...] + + # 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 +``` + 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..997285d 100644 --- a/package.xml +++ b/package.xml @@ -1,38 +1,51 @@ - + + 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 + + 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..a1a0ccf 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,21 @@ 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 + geometry_msgs + ) +ENDIF(COLCON_BUILD) + IF(TORCH_FOUND) SET(LIBRARIES @@ -141,31 +168,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 +232,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..5f8f930 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" 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..852646c --- /dev/null +++ b/src/ros2/FindObjectROS.cpp @@ -0,0 +1,362 @@ +/* +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 +#include + +#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(rclcpp::ReliabilityPolicy::Reliable)); + pubStamped_ = node->create_publisher("objectsStamped", rclcpp::QoS(1).reliability(rclcpp::ReliabilityPolicy::Reliable)); + pubInfo_ = node->create_publisher("info", rclcpp::QoS(1).reliability(rclcpp::ReliabilityPolicy::Reliable)); + + 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(); +} From 239caa29065bf23c207d3ddebfebff5c5eb0617a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Dec 2022 23:43:38 -0800 Subject: [PATCH 02/11] fixed build without ros --- CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0317f9f..01b6834 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,10 @@ ENDIF() MESSAGE(STATUS "CATKIN_BUILD=${CATKIN_BUILD}") MESSAGE(STATUS "COLCON_BUILD=${COLCON_BUILD}") -SET(ROS_BUILD CATKIN_BUILD OR COLCON_BUILD) +SET(ROS_BUILD FALSE) +IF(CATKIN_BUILD OR COLCON_BUILD) + SET(ROS_BUILD TRUE) +ENDIF() IF(NOT ROS_BUILD) #Standalone build From eebb4411ce3881c4dc2c08a0d6775aaa93549d79 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Dec 2022 23:49:38 -0800 Subject: [PATCH 03/11] Fixed ros1 build --- CMakeLists.txt | 4 ++-- src/ros1/FindObjectROS.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 01b6834..59b2013 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -380,7 +380,7 @@ ELSEIF(CATKIN_BUILD) ## 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 @@ -407,7 +407,7 @@ ELSEIF(CATKIN_BUILD) ## 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 ) diff --git a/src/ros1/FindObjectROS.cpp b/src/ros1/FindObjectROS.cpp index 5f8f930..d814534 100644 --- a/src/ros1/FindObjectROS.cpp +++ b/src/ros1/FindObjectROS.cpp @@ -208,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()); @@ -252,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); From dee5826d1fb5890b4736837d90b70e80d33f11bf Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Dec 2022 23:59:32 -0800 Subject: [PATCH 04/11] Added worflows --- .github/workflows/cmake.yml | 38 ++++++++++++++++++++++++ .github/workflows/ros1.yml | 59 +++++++++++++++++++++++++++++++++++++ .github/workflows/ros2.yml | 58 ++++++++++++++++++++++++++++++++++++ 3 files changed, 155 insertions(+) create mode 100644 .github/workflows/cmake.yml create mode 100644 .github/workflows/ros1.yml create mode 100644 .github/workflows/ros2.yml diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..ab88da5 --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,38 @@ +name: CMake + +on: + push: + branches: + - '**' + pull_request: + branches: + - '**' + +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 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..951e31a --- /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 + + - 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..f5243fb --- /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 + + - 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+ From fe502d5a0fc11a7ea8ab571226288b1a8927e83d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:03:46 -0800 Subject: [PATCH 05/11] Updated workflows --- .github/workflows/cmake.yml | 8 +++----- .github/workflows/ros1.yml | 2 +- .github/workflows/ros2.yml | 2 +- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index ab88da5..273581a 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,11 +2,9 @@ name: CMake on: push: - branches: - - '**' + branches: [ master ] pull_request: - branches: - - '**' + branches: [ master ] env: BUILD_TYPE: Release @@ -24,7 +22,7 @@ jobs: run: | DEBIAN_FRONTEND=noninteractive sudo apt-get update - sudo apt-get -y install libopencv-dev git cmake software-properties-common + sudo apt-get -y install libopencv-dev qtbase5-dev git cmake software-properties-common - uses: actions/checkout@v2 diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml index 951e31a..bcc9713 100644 --- a/.github/workflows/ros1.yml +++ b/.github/workflows/ros1.yml @@ -35,7 +35,7 @@ jobs: - name: Install dependencies run: | sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge + sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge qtbase5-dev - name: Setup catkin workspace run: | diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index f5243fb..364bebb 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -38,7 +38,7 @@ jobs: - name: Install dependencies run: | sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge + sudo apt-get -y install ros-${{ matrix.ros_distro }}-cv-bridge qtbase5-dev - name: Setup ros2 workspace run: | From 8526803d08321b290230b526ca1a27875a26c057 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:17:23 -0800 Subject: [PATCH 06/11] backward compatibility (foxy) --- src/ros2/FindObjectROS.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ros2/FindObjectROS.cpp b/src/ros2/FindObjectROS.cpp index 852646c..19389bf 100644 --- a/src/ros2/FindObjectROS.cpp +++ b/src/ros2/FindObjectROS.cpp @@ -50,9 +50,9 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) : 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(rclcpp::ReliabilityPolicy::Reliable)); - pubStamped_ = node->create_publisher("objectsStamped", rclcpp::QoS(1).reliability(rclcpp::ReliabilityPolicy::Reliable)); - pubInfo_ = node->create_publisher("info", rclcpp::QoS(1).reliability(rclcpp::ReliabilityPolicy::Reliable)); + 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))); } From 9cb2d09a328b56dc84338e9d490c5cdffa9b46bb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:23:35 -0800 Subject: [PATCH 07/11] readme: Updated CI status --- README.md | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 90079e1..ccd7d6b 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ ## find-object (standalone) -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) Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. @@ -92,3 +91,20 @@ A RGB-D camera is required. Example with Realsense D400 camera: $ ros2 run find_object_2d tf_example ``` +## CI Latest + + + + + + + + + + + + +
LinuxBuild Status
Build Status
Build Status +
WindowsBuild Status +
+ From f985fa4ff5645d2c3ddeaddcde75405cb8ea783a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:26:25 -0800 Subject: [PATCH 08/11] Fixing humble build --- src/ros2/FindObjectROS.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ros2/FindObjectROS.cpp b/src/ros2/FindObjectROS.cpp index 19389bf..cc6d2a5 100644 --- a/src/ros2/FindObjectROS.cpp +++ b/src/ros2/FindObjectROS.cpp @@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#ifdef PRE_ROS_HUMBLE #include +#else +#include +#endif #include From ade3e11e06d549a6a1f2731f697931b8995ba8d4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:34:20 -0800 Subject: [PATCH 09/11] Update README.md --- README.md | 93 +++++++++++++++++++++++-------------------------------- 1 file changed, 38 insertions(+), 55 deletions(-) diff --git a/README.md b/README.md index ccd7d6b..7946203 100644 --- a/README.md +++ b/README.md @@ -1,52 +1,52 @@ -## find-object (standalone) +# find-object + + + + + + + + + + + +
LinuxBuild Status
Build Status
Build Status +
WindowsBuild Status +
+ +## Standalone Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. -## find_object_2d (ROS1 package) +## ROS1: find_object_2d ### 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. - - * 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. + * 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 -# 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 ~/catkin_ws + git clone https://github.com/introlab/find-object.git src/find_object_2d + catkin_make ``` ### Run ```bash - $ roscore & + roscore # Launch your preferred usb camera driver - $ rosrun uvc_camera uvc_camera_node & - $ rosrun find_object_2d find_object_2d image:=image_raw + 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. -## find_object_2d (ROS2 package) +## ROS2: find_object_2d ### Install @@ -59,21 +59,21 @@ Source: ```bash # Install ROS Foxy/Galactic/Humble (colcon build): - $ cd ~/ros2_ws - $ git clone https://github.com/introlab/find-object.git src/find_object_2d - $ colcon build + 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 + 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 + 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: @@ -82,29 +82,12 @@ A RGB-D camera is required. Example with Realsense D400 camera: 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 + 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 + ros2 run find_object_2d tf_example ``` -## CI Latest - - - - - - - - - - - - -
LinuxBuild Status
Build Status
Build Status -
WindowsBuild Status -
- From 84285347028c20d2312bf4792dbace95eb3b3199 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:36:01 -0800 Subject: [PATCH 10/11] Update README.md --- README.md | 49 ++++++++++++++++++++++++------------------------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 7946203..7dcfbd4 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ ## Standalone Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information. -## ROS1: find_object_2d +## ROS1 ### Install @@ -32,21 +32,21 @@ Source: * 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 +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 +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: find_object_2d +## ROS2 ### Install @@ -58,36 +58,35 @@ To come... Source: ```bash -# Install ROS Foxy/Galactic/Humble (colcon build): - cd ~/ros2_ws - git clone https://github.com/introlab/find-object.git src/find_object_2d - colcon build +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 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 +# 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 +# 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 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 \ +# 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 +# Show 3D pose in camera frame: +ros2 run find_object_2d tf_example ``` From 8cf1bfcac20aaffec170e463d6ecc2cb702b80e1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 11 Dec 2022 00:39:46 -0800 Subject: [PATCH 11/11] Humble: missing tf2_geometry_msgs --- CMakeLists.txt | 1 + package.xml | 1 + src/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 59b2013..f2e52ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -439,6 +439,7 @@ ELSE() # COLCON_BUILD 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 diff --git a/package.xml b/package.xml index 997285d..d519aae 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ rclcpp tf2 tf2_ros + tf2_geometry_msgs ros_environment builtin_interfaces diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a1a0ccf..c6a95a6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -135,6 +135,7 @@ IF(COLCON_BUILD) message_filters tf2 tf2_ros + tf2_geometry_msgs geometry_msgs ) ENDIF(COLCON_BUILD)