matlabbe
GitHub
commit
c2f47bdf2b
36
.github/workflows/cmake.yml
vendored
Normal file
36
.github/workflows/cmake.yml
vendored
Normal file
@ -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}}
|
||||
|
||||
|
||||
59
.github/workflows/ros1.yml
vendored
Normal file
59
.github/workflows/ros1.yml
vendored
Normal file
@ -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
|
||||
58
.github/workflows/ros2.yml
vendored
Normal file
58
.github/workflows/ros2.yml
vendored
Normal file
@ -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+
|
||||
@ -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()
|
||||
|
||||
|
||||
100
README.md
100
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
|
||||
|
||||
<table>
|
||||
<tbody>
|
||||
<tr>
|
||||
<td>Linux</td>
|
||||
<td><a href="https://github.com/introlab/find-object/actions/workflows/cmake.yml"><img src="https://github.com/introlab/find-object/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/find-object/actions/workflows/ros1.yml"><img src="https://github.com/introlab/find-object/actions/workflows/ros1.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/find-object/actions/workflows/ros2.yml"><img src="https://github.com/introlab/find-object/actions/workflows/ros2.yml/badge.svg" alt="Build Status"/>
|
||||
</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Windows</td>
|
||||
<td><a href="https://ci.appveyor.com/project/matlabbe/find-object/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hn51r6p5c0peqctb/branch/master?svg=true" alt="Build Status"/>
|
||||
</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
## Standalone
|
||||
Find-Object project, visit the [home page](http://introlab.github.io/find-object/) for more information.
|
||||
|
||||
## find_object_2d (ROS package)
|
||||
## ROS1
|
||||
|
||||
### Install
|
||||
|
||||
Binaries:
|
||||
```bash
|
||||
# ROS Kinetic:
|
||||
$ sudo apt-get install ros-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 &
|
||||
# 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
|
||||
|
||||
### Install
|
||||
|
||||
Binaries:
|
||||
```bash
|
||||
To come...
|
||||
```
|
||||
|
||||
Source:
|
||||
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
git clone https://github.com/introlab/find-object.git src/find_object_2d
|
||||
colcon build
|
||||
```
|
||||
|
||||
### Run
|
||||
```bash
|
||||
# Launch your preferred usb camera driver
|
||||
ros2 launch realsense2_camera rs_launch.py
|
||||
|
||||
# Launch find_object_2d node:
|
||||
ros2 launch find_object_2d find_object_2d.launch.py image:=/camera/color/image_raw
|
||||
|
||||
# Draw objects detected on an image:
|
||||
ros2 run find_object_2d print_objects_detected --ros-args -r image:=/camera/color/image_raw
|
||||
```
|
||||
#### 3D Pose (TF)
|
||||
A RGB-D camera is required. Example with Realsense D400 camera:
|
||||
```bash
|
||||
# Launch your preferred usb camera driver
|
||||
ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
|
||||
|
||||
# Launch find_object_2d node:
|
||||
ros2 launch find_object_2d find_object_3d.launch.py \
|
||||
rgb_topic:=/camera/color/image_raw \
|
||||
depth_topic:=/camera/aligned_depth_to_color/image_raw \
|
||||
camera_info_topic:=/camera/color/camera_info
|
||||
|
||||
# Show 3D pose in camera frame:
|
||||
ros2 run find_object_2d tf_example
|
||||
```
|
||||
|
||||
|
||||
30
launch/ros2/find_object_2d.launch.py
Normal file
30
launch/ros2/find_object_2d.launch.py
Normal file
@ -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'))]),
|
||||
])
|
||||
42
launch/ros2/find_object_3d.launch.py
Normal file
42
launch/ros2/find_object_3d.launch.py
Normal file
@ -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'))]),
|
||||
])
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
54
package.xml
54
package.xml
@ -1,38 +1,52 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>find_object_2d</name>
|
||||
<version>0.6.4</version>
|
||||
<description>The find_object_2d package</description>
|
||||
|
||||
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
<license>BSD</license>
|
||||
<url type="website">http://find-object.googlecode.com</url>
|
||||
<author email="matlabbe@gmail.com">Mathieu Labbe</author>
|
||||
<url type="bugtracker">https://github.com/introlab/find-object/issues</url>
|
||||
<url type="repository">https://github.com/introlab/find-object</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
|
||||
|
||||
<depend condition="$ROS_VERSION == 1">roscpp</depend>
|
||||
<depend condition="$ROS_VERSION == 1">tf</depend>
|
||||
<depend condition="$ROS_VERSION == 2">rclcpp</depend>
|
||||
<depend condition="$ROS_VERSION == 2">tf2</depend>
|
||||
<depend condition="$ROS_VERSION == 2">tf2_ros</depend>
|
||||
<depend condition="$ROS_VERSION == 2">tf2_geometry_msgs</depend>
|
||||
|
||||
<build_depend condition="$ROS_VERSION == 2">ros_environment</build_depend>
|
||||
<depend condition="$ROS_VERSION == 2">builtin_interfaces</depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">rosidl_default_generators</build_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
|
||||
<member_of_group condition="$ROS_VERSION == 2">rosidl_interface_packages</member_of_group>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
|
||||
<build_depend>qtbase5-dev</build_depend>
|
||||
<build_depend>cv_bridge</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>image_transport</build_depend>
|
||||
<build_depend>message_filters</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>qtbase5-dev</run_depend>
|
||||
<run_depend>cv_bridge</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>std_srvs</run_depend>
|
||||
<run_depend>image_transport</run_depend>
|
||||
<run_depend>message_filters</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
|
||||
<exec_depend>qtbase5-dev</exec_depend>
|
||||
<exec_depend>cv_bridge</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>image_transport</exec_depend>
|
||||
<exec_depend>message_filters</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "CameraROS.h"
|
||||
#include "find_object/Settings.h"
|
||||
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include "CameraROS.h"
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
|
||||
using namespace find_object;
|
||||
@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "FindObjectROS.h"
|
||||
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include "find_object_2d/ObjectsStamped.h"
|
||||
#include "find_object_2d/DetectionInfo.h"
|
||||
@ -209,7 +208,7 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const Heade
|
||||
infoMsg.ids.resize(info.objDetected_.size());
|
||||
infoMsg.widths.resize(info.objDetected_.size());
|
||||
infoMsg.heights.resize(info.objDetected_.size());
|
||||
infoMsg.filePaths.resize(info.objDetected_.size());
|
||||
infoMsg.file_paths.resize(info.objDetected_.size());
|
||||
infoMsg.inliers.resize(info.objDetected_.size());
|
||||
infoMsg.outliers.resize(info.objDetected_.size());
|
||||
infoMsg.homographies.resize(info.objDetected_.size());
|
||||
@ -253,7 +252,7 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const Heade
|
||||
infoMsg.ids[infoIndex].data = iter.key();
|
||||
infoMsg.widths[infoIndex].data = iterSizes->width();
|
||||
infoMsg.heights[infoIndex].data = iterSizes->height();
|
||||
infoMsg.filePaths[infoIndex].data = iterFilePaths.value().toStdString();
|
||||
infoMsg.file_paths[infoIndex].data = iterFilePaths.value().toStdString();
|
||||
infoMsg.inliers[infoIndex].data = iterInliers.value();
|
||||
infoMsg.outliers[infoIndex].data = iterOutliers.value();
|
||||
infoMsg.homographies[infoIndex].data.resize(9);
|
||||
@ -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 <ros1/CameraROS.h>
|
||||
#include <ros1/FindObjectROS.h>
|
||||
#include <QApplication>
|
||||
#include <QDir>
|
||||
#include "find_object/MainWindow.h"
|
||||
188
src/ros2/CameraROS.cpp
Normal file
188
src/ros2/CameraROS.cpp
Normal file
@ -0,0 +1,188 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "CameraROS.h"
|
||||
#include "find_object/Settings.h"
|
||||
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
using namespace find_object;
|
||||
|
||||
CameraROS::CameraROS(bool subscribeDepth, rclcpp::Node * node) :
|
||||
node_(node),
|
||||
subscribeDepth_(subscribeDepth),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
{
|
||||
qRegisterMetaType<rclcpp::Time>("ros::Time");
|
||||
qRegisterMetaType<cv::Mat>("cv::Mat");
|
||||
|
||||
if(!subscribeDepth_)
|
||||
{
|
||||
image_transport::TransportHints hints(node);
|
||||
imageSub_ = image_transport::create_subscription(node, "image", std::bind(&CameraROS::imgReceivedCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
}
|
||||
else
|
||||
{
|
||||
int queueSize = 10;
|
||||
bool approxSync = true;
|
||||
queueSize = node->declare_parameter("queue_size", queueSize);
|
||||
approxSync = node->declare_parameter("approx_sync", approxSync);
|
||||
RCLCPP_INFO(node->get_logger(), "find_object_ros: queue_size = %d", queueSize);
|
||||
RCLCPP_INFO(node->get_logger(), "find_object_ros: approx_sync = %s", approxSync?"true":"false");
|
||||
|
||||
|
||||
image_transport::TransportHints hints(node);
|
||||
rgbSub_.subscribe(node, "rgb/image_rect_color", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
depthSub_.subscribe(node, "depth_registered/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(node, "depth_registered/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
|
||||
approxSync_->registerCallback(std::bind(&CameraROS::imgDepthReceivedCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
|
||||
exactSync_->registerCallback(std::bind(&CameraROS::imgDepthReceivedCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
}
|
||||
}
|
||||
CameraROS::~CameraROS()
|
||||
{
|
||||
delete approxSync_;
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
void CameraROS::setupExecutor(std::shared_ptr<rclcpp::Node> node)
|
||||
{
|
||||
executor_.add_node(node);
|
||||
}
|
||||
|
||||
QStringList CameraROS::subscribedTopics() const
|
||||
{
|
||||
QStringList topics;
|
||||
if(subscribeDepth_)
|
||||
{
|
||||
topics.append(rgbSub_.getTopic().c_str());
|
||||
topics.append(depthSub_.getTopic().c_str());
|
||||
topics.append(cameraInfoSub_.getSubscriber()->get_topic_name());
|
||||
}
|
||||
else
|
||||
{
|
||||
topics.append(imageSub_.getTopic().c_str());
|
||||
}
|
||||
return topics;
|
||||
}
|
||||
|
||||
bool CameraROS::start()
|
||||
{
|
||||
this->startTimer();
|
||||
return true;
|
||||
}
|
||||
|
||||
void CameraROS::stop()
|
||||
{
|
||||
this->stopTimer();
|
||||
}
|
||||
|
||||
void CameraROS::takeImage()
|
||||
{
|
||||
executor_.spin_some();
|
||||
}
|
||||
|
||||
void CameraROS::imgReceivedCallback(const sensor_msgs::msg::Image::ConstSharedPtr msg)
|
||||
{
|
||||
if(msg->data.size())
|
||||
{
|
||||
cv::Mat image;
|
||||
cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(msg);
|
||||
try
|
||||
{
|
||||
if(msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
msg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
|
||||
{
|
||||
image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
|
||||
}
|
||||
else
|
||||
{
|
||||
image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
|
||||
}
|
||||
|
||||
Q_EMIT imageReceived(image, Header(msg->header.frame_id.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec), cv::Mat(), 0.0f);
|
||||
}
|
||||
catch(const cv_bridge::Exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", msg->encoding.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CameraROS::imgDepthReceivedCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
|
||||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Depth image type must be 32FC1 or 16UC1");
|
||||
return;
|
||||
}
|
||||
|
||||
if(rgbMsg->data.size())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg);
|
||||
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
|
||||
float depthConstant = 1.0f/cameraInfoMsg->k[4];
|
||||
|
||||
cv::Mat image;
|
||||
cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(rgbMsg);
|
||||
try
|
||||
{
|
||||
if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
|
||||
rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
|
||||
{
|
||||
image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
|
||||
}
|
||||
else
|
||||
{
|
||||
image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
|
||||
}
|
||||
|
||||
Q_EMIT imageReceived(image, Header(rgbMsg->header.frame_id.c_str(), rgbMsg->header.stamp.sec, rgbMsg->header.stamp.nanosec), ptrDepth->image, depthConstant);
|
||||
}
|
||||
catch(const cv_bridge::Exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", rgbMsg->encoding.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
93
src/ros2/CameraROS.h
Normal file
93
src/ros2/CameraROS.h
Normal file
@ -0,0 +1,93 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef CAMERAROS_H_
|
||||
#define CAMERAROS_H_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
|
||||
#include "find_object/Camera.h"
|
||||
#include <QtCore/QStringList>
|
||||
|
||||
class CameraROS : public find_object::Camera {
|
||||
Q_OBJECT
|
||||
public:
|
||||
CameraROS(bool subscribeDepth, rclcpp::Node * node);
|
||||
virtual ~CameraROS();
|
||||
void setupExecutor(std::shared_ptr<rclcpp::Node> node);
|
||||
|
||||
virtual bool start();
|
||||
virtual void stop();
|
||||
|
||||
QStringList subscribedTopics() const;
|
||||
|
||||
private Q_SLOTS:
|
||||
virtual void takeImage();
|
||||
|
||||
private:
|
||||
void imgReceivedCallback(const sensor_msgs::msg::Image::ConstSharedPtr msg);
|
||||
void imgDepthReceivedCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
rclcpp::executors::SingleThreadedExecutor executor_;
|
||||
bool subscribeDepth_;
|
||||
image_transport::Subscriber imageSub_;
|
||||
|
||||
image_transport::SubscriberFilter rgbSub_;
|
||||
image_transport::SubscriberFilter depthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::msg::Image,
|
||||
sensor_msgs::msg::Image,
|
||||
sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::msg::Image,
|
||||
sensor_msgs::msg::Image,
|
||||
sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
};
|
||||
|
||||
#endif /* CAMERAROS_H_ */
|
||||
366
src/ros2/FindObjectROS.cpp
Normal file
366
src/ros2/FindObjectROS.cpp
Normal file
@ -0,0 +1,366 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "FindObjectROS.h"
|
||||
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/LinearMath/Vector3.h>
|
||||
#include <tf2/LinearMath/Matrix3x3.h>
|
||||
#ifdef PRE_ROS_HUMBLE
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#else
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#endif
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace find_object;
|
||||
|
||||
FindObjectROS::FindObjectROS(rclcpp::Node * node) :
|
||||
FindObject(true),
|
||||
node_(node),
|
||||
objFramePrefix_("object"),
|
||||
usePnP_(true)
|
||||
{
|
||||
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
|
||||
|
||||
objFramePrefix_ = node->declare_parameter("object_prefix", objFramePrefix_);
|
||||
usePnP_ = node->declare_parameter("pnp", usePnP_);
|
||||
RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str());
|
||||
RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false");
|
||||
|
||||
pub_ = node->create_publisher<std_msgs::msg::Float32MultiArray>("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
|
||||
pubStamped_ = node->create_publisher<find_object_2d::msg::ObjectsStamped>("objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
|
||||
pubInfo_ = node->create_publisher<find_object_2d::msg::DetectionInfo>("info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
|
||||
|
||||
this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
|
||||
}
|
||||
|
||||
void FindObjectROS::publish(const find_object::DetectionInfo & info, const Header & header, const cv::Mat & depth, float depthConstant)
|
||||
{
|
||||
// send tf before the message
|
||||
if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::TransformStamped> transforms;
|
||||
char multiSubId = 'b';
|
||||
int previousId = -1;
|
||||
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
|
||||
for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
|
||||
iter!=info.objDetected_.constEnd();
|
||||
++iter, ++iterSizes)
|
||||
{
|
||||
// get data
|
||||
int id = iter.key();
|
||||
float objectWidth = iterSizes->width();
|
||||
float objectHeight = iterSizes->height();
|
||||
|
||||
QString multiSuffix;
|
||||
if(id == previousId)
|
||||
{
|
||||
multiSuffix = QString("_") + multiSubId++;
|
||||
}
|
||||
else
|
||||
{
|
||||
multiSubId = 'b';
|
||||
}
|
||||
previousId = id;
|
||||
|
||||
// Find center of the object
|
||||
QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
|
||||
cv::Vec3f center3D = this->getDepth(depth,
|
||||
center.x()+0.5f, center.y()+0.5f,
|
||||
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
|
||||
1.0f/depthConstant, 1.0f/depthConstant);
|
||||
|
||||
cv::Vec3f axisEndX;
|
||||
cv::Vec3f axisEndY;
|
||||
if(!usePnP_)
|
||||
{
|
||||
QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
|
||||
axisEndX = this->getDepth(depth,
|
||||
xAxis.x()+0.5f, xAxis.y()+0.5f,
|
||||
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
|
||||
1.0f/depthConstant, 1.0f/depthConstant);
|
||||
|
||||
QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
|
||||
axisEndY = this->getDepth(depth,
|
||||
yAxis.x()+0.5f, yAxis.y()+0.5f,
|
||||
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
|
||||
1.0f/depthConstant, 1.0f/depthConstant);
|
||||
}
|
||||
|
||||
if((std::isfinite(center3D.val[2]) && usePnP_) ||
|
||||
(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
|
||||
std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
|
||||
std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2])))
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped transform;
|
||||
transform.transform.rotation.x=0;
|
||||
transform.transform.rotation.y=0;
|
||||
transform.transform.rotation.z=0;
|
||||
transform.transform.rotation.w=1;
|
||||
transform.transform.translation.x=0;
|
||||
transform.transform.translation.y=0;
|
||||
transform.transform.translation.z=0;
|
||||
transform.child_frame_id = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
|
||||
transform.header.frame_id = header.frameId_.toStdString();
|
||||
transform.header.stamp.sec = header.sec_;
|
||||
transform.header.stamp.nanosec = header.nsec_;
|
||||
|
||||
tf2::Quaternion q;
|
||||
if(usePnP_)
|
||||
{
|
||||
std::vector<cv::Point3f> objectPoints(4);
|
||||
std::vector<cv::Point2f> imagePoints(4);
|
||||
|
||||
objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
|
||||
objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
|
||||
objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
|
||||
objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
|
||||
|
||||
QPointF pt = iter->map(QPointF(0, 0));
|
||||
imagePoints[0] = cv::Point2f(pt.x(), pt.y());
|
||||
pt = iter->map(QPointF(objectWidth, 0));
|
||||
imagePoints[1] = cv::Point2f(pt.x(), pt.y());
|
||||
pt = iter->map(QPointF(objectWidth, objectHeight));
|
||||
imagePoints[2] = cv::Point2f(pt.x(), pt.y());
|
||||
pt = iter->map(QPointF(0, objectHeight));
|
||||
imagePoints[3] = cv::Point2f(pt.x(), pt.y());
|
||||
|
||||
cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
|
||||
cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
|
||||
cameraMatrix.at<double>(1,2) = float(depth.rows/2)-0.5f;
|
||||
cv::Mat distCoeffs;
|
||||
|
||||
cv::Mat rvec(1,3, CV_64FC1);
|
||||
cv::Mat tvec(1,3, CV_64FC1);
|
||||
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
|
||||
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(rvec, R);
|
||||
tf2::Matrix3x3 rotationMatrix(
|
||||
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
|
||||
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
|
||||
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
|
||||
rotationMatrix.getRotation(q);
|
||||
transform.transform.translation.x = tvec.at<double>(0)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.y = tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.z = tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(2));
|
||||
}
|
||||
else
|
||||
{
|
||||
tf2::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
|
||||
xAxis.normalize();
|
||||
tf2::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
|
||||
yAxis.normalize();
|
||||
tf2::Vector3 zAxis = xAxis.cross(yAxis);
|
||||
zAxis.normalize();
|
||||
tf2::Matrix3x3 rotationMatrix(
|
||||
xAxis.x(), yAxis.x() ,zAxis.x(),
|
||||
xAxis.y(), yAxis.y(), zAxis.y(),
|
||||
xAxis.z(), yAxis.z(), zAxis.z());
|
||||
rotationMatrix.getRotation(q);
|
||||
transform.transform.translation.x = center3D.val[0];
|
||||
transform.transform.translation.y = center3D.val[1];
|
||||
transform.transform.translation.z = center3D.val[2];
|
||||
}
|
||||
|
||||
// set x axis going front of the object, with z up and y left
|
||||
tf2::Quaternion q2;
|
||||
q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0);
|
||||
q *= q2;
|
||||
transform.transform.rotation = tf2::toMsg(q.normalized());
|
||||
transforms.push_back(transform);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(node_->get_logger(), "Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
|
||||
"(maybe object is too near of the camera or bad depth image)\n",
|
||||
id,
|
||||
center.x(), center.y(),
|
||||
QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
|
||||
}
|
||||
}
|
||||
if(transforms.size())
|
||||
{
|
||||
tfBroadcaster_->sendTransform(transforms);
|
||||
}
|
||||
}
|
||||
|
||||
if(pub_->get_subscription_count() || pubStamped_->get_subscription_count() || pubInfo_->get_subscription_count())
|
||||
{
|
||||
std_msgs::msg::Float32MultiArray msg;
|
||||
find_object_2d::msg::ObjectsStamped msgStamped;
|
||||
find_object_2d::msg::DetectionInfo infoMsg;
|
||||
if(pubInfo_->get_subscription_count())
|
||||
{
|
||||
infoMsg.ids.resize(info.objDetected_.size());
|
||||
infoMsg.widths.resize(info.objDetected_.size());
|
||||
infoMsg.heights.resize(info.objDetected_.size());
|
||||
infoMsg.file_paths.resize(info.objDetected_.size());
|
||||
infoMsg.inliers.resize(info.objDetected_.size());
|
||||
infoMsg.outliers.resize(info.objDetected_.size());
|
||||
infoMsg.homographies.resize(info.objDetected_.size());
|
||||
}
|
||||
msg.data = std::vector<float>(info.objDetected_.size()*12);
|
||||
msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
|
||||
|
||||
Q_ASSERT(info.objDetected_.size() == info.objDetectedSizes_.size() &&
|
||||
info.objDetected_.size() == info.objDetectedFilePaths_.size() &&
|
||||
info.objDetected_.size() == info.objDetectedInliersCount_.size() &&
|
||||
info.objDetected_.size() == info.objDetectedOutliersCount_.size());
|
||||
|
||||
int infoIndex=0;
|
||||
int i=0;
|
||||
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
|
||||
QMultiMap<int, QString>::const_iterator iterFilePaths=info.objDetectedFilePaths_.constBegin();
|
||||
QMultiMap<int, int>::const_iterator iterInliers=info.objDetectedInliersCount_.constBegin();
|
||||
QMultiMap<int, int>::const_iterator iterOutliers=info.objDetectedOutliersCount_.constBegin();
|
||||
for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
|
||||
iter!=info.objDetected_.constEnd();
|
||||
++iter, ++iterSizes, ++iterFilePaths, ++infoIndex, ++iterInliers, ++iterOutliers)
|
||||
{
|
||||
if(pub_->get_subscription_count() || pubStamped_->get_subscription_count())
|
||||
{
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
|
||||
msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
|
||||
}
|
||||
|
||||
if(pubInfo_->get_subscription_count())
|
||||
{
|
||||
infoMsg.ids[infoIndex].data = iter.key();
|
||||
infoMsg.widths[infoIndex].data = iterSizes->width();
|
||||
infoMsg.heights[infoIndex].data = iterSizes->height();
|
||||
infoMsg.file_paths[infoIndex].data = iterFilePaths.value().toStdString();
|
||||
infoMsg.inliers[infoIndex].data = iterInliers.value();
|
||||
infoMsg.outliers[infoIndex].data = iterOutliers.value();
|
||||
infoMsg.homographies[infoIndex].data.resize(9);
|
||||
infoMsg.homographies[infoIndex].data[0] = iter->m11();
|
||||
infoMsg.homographies[infoIndex].data[1] = iter->m12();
|
||||
infoMsg.homographies[infoIndex].data[2] = iter->m13();
|
||||
infoMsg.homographies[infoIndex].data[3] = iter->m21();
|
||||
infoMsg.homographies[infoIndex].data[4] = iter->m22();
|
||||
infoMsg.homographies[infoIndex].data[5] = iter->m23();
|
||||
infoMsg.homographies[infoIndex].data[6] = iter->m31();
|
||||
infoMsg.homographies[infoIndex].data[7] = iter->m32();
|
||||
infoMsg.homographies[infoIndex].data[8] = iter->m33();
|
||||
}
|
||||
}
|
||||
if(pub_->get_subscription_count())
|
||||
{
|
||||
pub_->publish(msg);
|
||||
}
|
||||
if(pubStamped_->get_subscription_count())
|
||||
{
|
||||
// use same header as the input image (for synchronization and frame reference)
|
||||
msgStamped.header.frame_id = header.frameId_.toStdString();
|
||||
msgStamped.header.stamp.sec = header.sec_;
|
||||
msgStamped.header.stamp.nanosec = header.nsec_;
|
||||
pubStamped_->publish(msgStamped);
|
||||
}
|
||||
if(pubInfo_->get_subscription_count())
|
||||
{
|
||||
// use same header as the input image (for synchronization and frame reference)
|
||||
infoMsg.header.frame_id = header.frameId_.toStdString();
|
||||
infoMsg.header.stamp.sec = header.sec_;
|
||||
infoMsg.header.stamp.nanosec = header.nsec_;
|
||||
pubInfo_->publish(infoMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
|
||||
int x, int y,
|
||||
float cx, float cy,
|
||||
float fx, float fy)
|
||||
{
|
||||
if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
|
||||
x, y,
|
||||
depthImage.cols, depthImage.rows);
|
||||
return cv::Vec3f(
|
||||
std::numeric_limits<float>::quiet_NaN (),
|
||||
std::numeric_limits<float>::quiet_NaN (),
|
||||
std::numeric_limits<float>::quiet_NaN ());
|
||||
}
|
||||
|
||||
|
||||
cv::Vec3f pt;
|
||||
|
||||
// Use correct principal point from calibration
|
||||
float center_x = cx; //cameraInfo.K.at(2)
|
||||
float center_y = cy; //cameraInfo.K.at(5)
|
||||
|
||||
bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
|
||||
|
||||
// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
|
||||
float unit_scaling = isInMM?0.001f:1.0f;
|
||||
float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
|
||||
float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
|
||||
float bad_point = std::numeric_limits<float>::quiet_NaN ();
|
||||
|
||||
float depth;
|
||||
bool isValid;
|
||||
if(isInMM)
|
||||
{
|
||||
depth = (float)depthImage.at<uint16_t>(y,x);
|
||||
isValid = depth != 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
depth = depthImage.at<float>(y,x);
|
||||
isValid = std::isfinite(depth) && depth > 0.0f;
|
||||
}
|
||||
|
||||
// Check for invalid measurements
|
||||
if (!isValid)
|
||||
{
|
||||
pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Fill in XYZ
|
||||
pt.val[0] = (float(x) - center_x) * depth * constant_x;
|
||||
pt.val[1] = (float(y) - center_y) * depth * constant_y;
|
||||
pt.val[2] = depth*unit_scaling;
|
||||
}
|
||||
return pt;
|
||||
}
|
||||
75
src/ros2/FindObjectROS.h
Normal file
75
src/ros2/FindObjectROS.h
Normal file
@ -0,0 +1,75 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef FINDOBJECTROS_H_
|
||||
#define FINDOBJECTROS_H_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include "find_object/FindObject.h"
|
||||
|
||||
#include <std_msgs/msg/float32_multi_array.hpp>
|
||||
#include "find_object_2d/msg/objects_stamped.hpp"
|
||||
#include "find_object_2d/msg/detection_info.hpp"
|
||||
|
||||
#include <QtCore/QObject>
|
||||
#include <QtCore/QMultiMap>
|
||||
#include <QtCore/QPair>
|
||||
#include <QtCore/QRect>
|
||||
#include <QtGui/QTransform>
|
||||
|
||||
class FindObjectROS : public find_object::FindObject
|
||||
{
|
||||
Q_OBJECT;
|
||||
|
||||
public:
|
||||
FindObjectROS(rclcpp::Node * node);
|
||||
virtual ~FindObjectROS() {}
|
||||
|
||||
public Q_SLOTS:
|
||||
void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
|
||||
|
||||
private:
|
||||
cv::Vec3f getDepth(const cv::Mat & depthImage,
|
||||
int x, int y,
|
||||
float cx, float cy,
|
||||
float fx, float fy);
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr pub_;
|
||||
rclcpp::Publisher<find_object_2d::msg::ObjectsStamped>::SharedPtr pubStamped_;
|
||||
rclcpp::Publisher<find_object_2d::msg::DetectionInfo>::SharedPtr pubInfo_;
|
||||
|
||||
std::string objFramePrefix_;
|
||||
bool usePnP_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
|
||||
|
||||
};
|
||||
|
||||
#endif /* FINDOBJECTROS_H_ */
|
||||
217
src/ros2/find_object_2d_node.cpp
Normal file
217
src/ros2/find_object_2d_node.cpp
Normal file
@ -0,0 +1,217 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "CameraROS.h"
|
||||
#include "FindObjectROS.h"
|
||||
|
||||
#include <QApplication>
|
||||
#include <QDir>
|
||||
#include "find_object/MainWindow.h"
|
||||
#include "ParametersToolBox.h"
|
||||
#include "find_object/Settings.h"
|
||||
#include <signal.h>
|
||||
|
||||
void my_handler_gui(int s){
|
||||
QApplication::closeAllWindows();
|
||||
QApplication::quit();
|
||||
}
|
||||
void my_handler(int s){
|
||||
QCoreApplication::quit();
|
||||
}
|
||||
|
||||
void setupQuitSignal(bool gui)
|
||||
{
|
||||
// Catch ctrl-c to close the gui
|
||||
struct sigaction sigIntHandler;
|
||||
if(gui)
|
||||
{
|
||||
sigIntHandler.sa_handler = my_handler_gui;
|
||||
}
|
||||
else
|
||||
{
|
||||
sigIntHandler.sa_handler = my_handler;
|
||||
}
|
||||
sigemptyset(&sigIntHandler.sa_mask);
|
||||
sigIntHandler.sa_flags = 0;
|
||||
sigaction(SIGINT, &sigIntHandler, NULL);
|
||||
}
|
||||
|
||||
class FindObjectNode : public rclcpp::Node {
|
||||
|
||||
public:
|
||||
FindObjectNode() :
|
||||
rclcpp::Node("find_object_2d"),
|
||||
findObjectROS_(0),
|
||||
camera_(0),
|
||||
gui_(true)
|
||||
{
|
||||
std::string objectsPath;
|
||||
std::string sessionPath;
|
||||
std::string settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
|
||||
bool subscribeDepth = false;
|
||||
|
||||
gui_ = this->declare_parameter("gui", gui_);
|
||||
objectsPath = this->declare_parameter("objects_path", objectsPath);
|
||||
sessionPath = this->declare_parameter("session_path", sessionPath);
|
||||
settingsPath = this->declare_parameter("settings_path", settingsPath);
|
||||
subscribeDepth = this->declare_parameter("subscribe_depth", subscribeDepth);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "gui=%d", gui_?1:0);
|
||||
RCLCPP_INFO(this->get_logger(), "objects_path=%s", objectsPath.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "session_path=%s", sessionPath.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "settings_path=%s", settingsPath.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "subscribe_depth = %s", subscribeDepth?"true":"false");
|
||||
|
||||
if(settingsPath.empty())
|
||||
{
|
||||
settingsPath = QDir::homePath().append("/.ros/find_object_2d.ini").toStdString();
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!sessionPath.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "\"settings_path\" parameter is ignored when \"session_path\" is set.");
|
||||
}
|
||||
|
||||
QString path = settingsPath.c_str();
|
||||
if(path.contains('~'))
|
||||
{
|
||||
path.replace('~', QDir::homePath());
|
||||
settingsPath = path.toStdString();
|
||||
}
|
||||
}
|
||||
|
||||
// Load settings, should be loaded before creating other objects
|
||||
find_object::Settings::init(settingsPath.c_str());
|
||||
|
||||
findObjectROS_ = new FindObjectROS(this);
|
||||
if(!sessionPath.empty())
|
||||
{
|
||||
if(!objectsPath.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "\"objects_path\" parameter is ignored when \"session_path\" is set.");
|
||||
}
|
||||
if(!findObjectROS_->loadSession(sessionPath.c_str()))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to load session \"%s\"", sessionPath.c_str());
|
||||
}
|
||||
}
|
||||
else if(!objectsPath.empty())
|
||||
{
|
||||
QString path = objectsPath.c_str();
|
||||
if(path.contains('~'))
|
||||
{
|
||||
path.replace('~', QDir::homePath());
|
||||
}
|
||||
if(!findObjectROS_->loadObjects(path))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "No objects loaded from path \"%s\"", path.toStdString().c_str());
|
||||
}
|
||||
}
|
||||
camera_ = new CameraROS(subscribeDepth, this);
|
||||
|
||||
// Catch ctrl-c to close the gui
|
||||
setupQuitSignal(gui_);
|
||||
}
|
||||
|
||||
void exec(int argc, char ** argv, std::shared_ptr<rclcpp::Node> node)
|
||||
{
|
||||
camera_->setupExecutor(node);
|
||||
if(gui_)
|
||||
{
|
||||
QApplication app(argc, argv);
|
||||
find_object::MainWindow mainWindow(findObjectROS_, camera_); // take ownership
|
||||
|
||||
QObject::connect(
|
||||
&mainWindow,
|
||||
SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)),
|
||||
findObjectROS_,
|
||||
SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
|
||||
|
||||
QStringList topics = camera_->subscribedTopics();
|
||||
if(topics.size() == 1)
|
||||
{
|
||||
mainWindow.setSourceImageText(mainWindow.tr(
|
||||
"<qt>Find-Object subscribed to <b>%1</b> topic.<br/>"
|
||||
"You can remap the topic when starting the node: <br/>\"rosrun find_object_2d find_object_2d image:=your/image/topic\".<br/>"
|
||||
"</qt>").arg(topics.first()));
|
||||
}
|
||||
else if(topics.size() == 3)
|
||||
{
|
||||
mainWindow.setSourceImageText(mainWindow.tr(
|
||||
"<qt>Find-Object subscribed to : <br/> <b>%1</b> <br/> <b>%2</b> <br/> <b>%3</b><br/>"
|
||||
"</qt>").arg(topics.at(0)).arg(topics.at(1)).arg(topics.at(2)));
|
||||
}
|
||||
mainWindow.show();
|
||||
app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) );
|
||||
|
||||
// mainWindow has ownership
|
||||
findObjectROS_ = 0;
|
||||
camera_ = 0;
|
||||
|
||||
// loop
|
||||
mainWindow.startProcessing();
|
||||
app.exec();
|
||||
find_object::Settings::saveSettings();
|
||||
}
|
||||
else
|
||||
{
|
||||
QCoreApplication app(argc, argv);
|
||||
|
||||
// connect stuff:
|
||||
QObject::connect(camera_, SIGNAL(imageReceived(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)), findObjectROS_, SLOT(detect(const cv::Mat &, const find_object::Header &, const cv::Mat &, float)));
|
||||
|
||||
//loop
|
||||
camera_->start();
|
||||
app.exec();
|
||||
|
||||
delete camera_;
|
||||
camera_=0;
|
||||
delete findObjectROS_;
|
||||
findObjectROS_=0;
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~FindObjectNode()
|
||||
{
|
||||
delete findObjectROS_;
|
||||
delete camera_;
|
||||
}
|
||||
|
||||
private:
|
||||
FindObjectROS * findObjectROS_;
|
||||
CameraROS * camera_;
|
||||
bool gui_;
|
||||
};
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<FindObjectNode>();
|
||||
node->exec(argc, argv, node);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
183
src/ros2/print_objects_detected_node.cpp
Normal file
183
src/ros2/print_objects_detected_node.cpp
Normal file
@ -0,0 +1,183 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <find_object_2d/msg/objects_stamped.hpp>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <QTransform>
|
||||
#include <QColor>
|
||||
|
||||
class PrintObjects: public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PrintObjects() :
|
||||
Node("objects_detected")
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
|
||||
imagePub_ = image_transport::create_publisher(this, "image_with_objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
|
||||
// Simple subscriber
|
||||
sub_ = create_subscription<std_msgs::msg::Float32MultiArray>("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1), std::bind(&PrintObjects::objectsDetectedCallback, this, std::placeholders::_1));
|
||||
|
||||
// Synchronized image + objects example
|
||||
imageSub_.subscribe(this, "image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
objectsSub_.subscribe(this, "objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
|
||||
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(10), imageSub_, objectsSub_);
|
||||
exactSync_->registerCallback(std::bind(&PrintObjects::imageObjectsDetectedCallback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
virtual ~PrintObjects()
|
||||
{
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
/**
|
||||
* IMPORTANT :
|
||||
* Parameter General/MirrorView must be false
|
||||
* Parameter Homography/homographyComputed must be true
|
||||
*/
|
||||
void objectsDetectedCallback(
|
||||
const std_msgs::msg::Float32MultiArray::ConstSharedPtr msg)
|
||||
{
|
||||
printf("---\n");
|
||||
const std::vector<float> & data = msg->data;
|
||||
if(data.size())
|
||||
{
|
||||
for(unsigned int i=0; i<data.size(); i+=12)
|
||||
{
|
||||
// get data
|
||||
int id = (int)data[i];
|
||||
float objectWidth = data[i+1];
|
||||
float objectHeight = data[i+2];
|
||||
|
||||
// Find corners Qt
|
||||
QTransform qtHomography(data[i+3], data[i+4], data[i+5],
|
||||
data[i+6], data[i+7], data[i+8],
|
||||
data[i+9], data[i+10], data[i+11]);
|
||||
|
||||
QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
|
||||
QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
|
||||
QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
|
||||
QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
|
||||
|
||||
printf("Object %d detected, Qt corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
|
||||
id,
|
||||
qtTopLeft.x(), qtTopLeft.y(),
|
||||
qtTopRight.x(), qtTopRight.y(),
|
||||
qtBottomLeft.x(), qtBottomLeft.y(),
|
||||
qtBottomRight.x(), qtBottomRight.y());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("No objects detected.\n");
|
||||
}
|
||||
}
|
||||
void imageObjectsDetectedCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const find_object_2d::msg::ObjectsStamped::ConstSharedPtr objectsMsg)
|
||||
{
|
||||
if(imagePub_.getNumSubscribers() > 0)
|
||||
{
|
||||
const std::vector<float> & data = objectsMsg->objects.data;
|
||||
if(data.size())
|
||||
{
|
||||
for(unsigned int i=0; i<data.size(); i+=12)
|
||||
{
|
||||
// get data
|
||||
int id = (int)data[i];
|
||||
float objectWidth = data[i+1];
|
||||
float objectHeight = data[i+2];
|
||||
|
||||
// Find corners OpenCV
|
||||
cv::Mat cvHomography(3, 3, CV_32F);
|
||||
cvHomography.at<float>(0,0) = data[i+3];
|
||||
cvHomography.at<float>(1,0) = data[i+4];
|
||||
cvHomography.at<float>(2,0) = data[i+5];
|
||||
cvHomography.at<float>(0,1) = data[i+6];
|
||||
cvHomography.at<float>(1,1) = data[i+7];
|
||||
cvHomography.at<float>(2,1) = data[i+8];
|
||||
cvHomography.at<float>(0,2) = data[i+9];
|
||||
cvHomography.at<float>(1,2) = data[i+10];
|
||||
cvHomography.at<float>(2,2) = data[i+11];
|
||||
std::vector<cv::Point2f> inPts, outPts;
|
||||
inPts.push_back(cv::Point2f(0,0));
|
||||
inPts.push_back(cv::Point2f(objectWidth,0));
|
||||
inPts.push_back(cv::Point2f(objectWidth,objectHeight));
|
||||
inPts.push_back(cv::Point2f(0,objectHeight));
|
||||
inPts.push_back(cv::Point2f(objectWidth/2,objectHeight/2));
|
||||
cv::perspectiveTransform(inPts, outPts, cvHomography);
|
||||
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageMsg);
|
||||
|
||||
cv_bridge::CvImage img;
|
||||
img = *imageDepthPtr;
|
||||
std::vector<cv::Point2i> outPtsInt;
|
||||
outPtsInt.push_back(outPts[0]);
|
||||
outPtsInt.push_back(outPts[1]);
|
||||
outPtsInt.push_back(outPts[2]);
|
||||
outPtsInt.push_back(outPts[3]);
|
||||
QColor color(QColor((Qt::GlobalColor)((id % 10 + 7)==Qt::yellow?Qt::darkYellow:(id % 10 + 7))));
|
||||
cv::Scalar cvColor(color.red(), color.green(), color.blue());
|
||||
cv::polylines(img.image, outPtsInt, true, cvColor, 3);
|
||||
cv::Point2i center = outPts[4];
|
||||
cv::putText(img.image, QString("(%1, %2)").arg(center.x).arg(center.y).toStdString(), center, cv::FONT_HERSHEY_SIMPLEX, 0.6, cvColor, 2);
|
||||
cv::circle(img.image, center, 1, cvColor, 3);
|
||||
imagePub_.publish(img.toImageMsg());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::msg::Image,
|
||||
find_object_2d::msg::ObjectsStamped> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
image_transport::Publisher imagePub_;
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
message_filters::Subscriber<find_object_2d::msg::ObjectsStamped> objectsSub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<PrintObjects>());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
122
src/ros2/tf_example_node.cpp
Normal file
122
src/ros2/tf_example_node.cpp
Normal file
@ -0,0 +1,122 @@
|
||||
/*
|
||||
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <find_object_2d/msg/objects_stamped.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <QtCore/QString>
|
||||
|
||||
class TfExample : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TfExample() :
|
||||
Node("tf_example_node"),
|
||||
objFramePrefix_("object")
|
||||
{
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
|
||||
targetFrameId_ = this->declare_parameter("target_frame_id", targetFrameId_);
|
||||
objFramePrefix_ = this->declare_parameter("object_prefix", objFramePrefix_);
|
||||
|
||||
subs_ = create_subscription<find_object_2d::msg::ObjectsStamped>("objectsStamped", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)1), std::bind(&TfExample::objectsDetectedCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
// Here I synchronize with the ObjectsStamped topic to
|
||||
// know when the TF is ready and for which objects
|
||||
void objectsDetectedCallback(const find_object_2d::msg::ObjectsStamped::ConstSharedPtr msg)
|
||||
{
|
||||
if(msg->objects.data.size())
|
||||
{
|
||||
std::string targetFrameId = targetFrameId_;
|
||||
if(targetFrameId.empty())
|
||||
{
|
||||
targetFrameId = msg->header.frame_id;
|
||||
}
|
||||
char multiSubId = 'b';
|
||||
int previousId = -1;
|
||||
for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
|
||||
{
|
||||
// get data
|
||||
int id = (int)msg->objects.data[i];
|
||||
|
||||
QString multiSuffix;
|
||||
if(id == previousId)
|
||||
{
|
||||
multiSuffix = QString("_") + multiSubId++;
|
||||
}
|
||||
else
|
||||
{
|
||||
multiSubId = 'b';
|
||||
}
|
||||
previousId = id;
|
||||
|
||||
// "object_1", "object_1_b", "object_1_c", "object_2"
|
||||
std::string objectFrameId = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
|
||||
|
||||
geometry_msgs::msg::TransformStamped pose;
|
||||
try
|
||||
{
|
||||
// Get transformation from "object_#" frame to target frame
|
||||
// The timestamp matches the one sent over TF
|
||||
pose = tfBuffer_->lookupTransform(targetFrameId, objectFrameId, tf2_ros::fromMsg(msg->header.stamp));
|
||||
}
|
||||
catch(tf2::TransformException & ex)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "%s",ex.what());
|
||||
continue;
|
||||
}
|
||||
|
||||
// Here "pose" is the position of the object "id" in target frame.
|
||||
RCLCPP_INFO(this->get_logger(), "%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
|
||||
objectFrameId.c_str(), targetFrameId.c_str(),
|
||||
pose.transform.translation.x, pose.transform.translation.y, pose.transform.translation.z,
|
||||
pose.transform.rotation.x, pose.transform.rotation.y, pose.transform.rotation.z, pose.transform.rotation.w);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::string targetFrameId_;
|
||||
std::string objFramePrefix_;
|
||||
rclcpp::Subscription<find_object_2d::msg::ObjectsStamped>::SharedPtr subs_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<TfExample>());
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user