From a235fa25a66362ffb7db6520ee81d8a81a54ad36 Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Tue, 27 Mar 2018 14:26:14 +0800 Subject: [PATCH 1/6] Building map demos in simulation environment --- orbslam2_demo/CMakeLists.txt | 199 ++++ orbslam2_demo/README.md | 8 + orbslam2_demo/launch/ros_orbslam2.launch | 7 + orbslam2_demo/launch/ros_orbslam2.launch~ | 6 + orbslam2_demo/package.xml | 68 ++ orbslam2_demo/param/Zdzn.yaml | 69 ++ ...46\347\273\206\346\255\245\351\252\244.md" | 93 ++ rtabmap_demo/CMakeLists.txt | 199 ++++ rtabmap_demo/README.md | 10 + rtabmap_demo/launch/config/appearance_gui.ini | 166 ++++ .../launch/config/demo_find_object.rviz | 332 +++++++ .../launch/config/demo_stereo_outdoor.rviz | 318 +++++++ rtabmap_demo/launch/config/find_object.ini | 136 +++ rtabmap_demo/launch/config/rgbd.rviz | 208 ++++ rtabmap_demo/launch/config/rgbd_gui.ini | 204 ++++ .../launch/config/rgbdslam_datasets.rviz | 291 ++++++ .../launch/config/rtabmap_robot_mapping.rviz | 306 ++++++ rtabmap_demo/launch/config/test_odometry.rviz | 240 +++++ .../launch/config/turtlebot_navigation.rviz | 446 +++++++++ .../launch/rtabmap_robot_mapping.launch | 73 ++ rtabmap_demo/package.xml | 68 ++ ...15\344\275\234\346\214\207\345\215\227.md" | 74 ++ rtabmap_demo/src/nodelets/data_odom_sync.cpp | 130 +++ rtabmap_demo/src/nodelets/data_throttle.cpp | 220 +++++ .../src/nodelets/disparity_to_depth.cpp | 140 +++ rtabmap_demo/src/nodelets/icp_odometry.cpp | 347 +++++++ .../src/nodelets/obstacles_detection.cpp | 411 ++++++++ .../src/nodelets/obstacles_detection_old.cpp | 357 +++++++ .../src/nodelets/point_cloud_aggregator.cpp | 112 +++ rtabmap_demo/src/nodelets/point_cloud_xyz.cpp | 377 ++++++++ .../src/nodelets/point_cloud_xyzrgb.cpp | 539 +++++++++++ .../src/nodelets/pointcloud_to_depthimage.cpp | 247 +++++ rtabmap_demo/src/nodelets/rgbd_odometry.cpp | 654 +++++++++++++ rtabmap_demo/src/nodelets/rgbd_sync.cpp | 243 +++++ .../src/nodelets/rgbdicp_odometry.cpp | 473 +++++++++ rtabmap_demo/src/nodelets/stereo_odometry.cpp | 347 +++++++ rtabmap_demo/src/nodelets/stereo_throttle.cpp | 246 +++++ rtabmap_demo/src/nodelets/undistort_depth.cpp | 121 +++ rtabmap_demo/src/rviz/InfoDisplay.cpp | 131 +++ rtabmap_demo/src/rviz/InfoDisplay.h | 70 ++ rtabmap_demo/src/rviz/MapCloudDisplay.cpp | 901 ++++++++++++++++++ rtabmap_demo/src/rviz/MapCloudDisplay.h | 196 ++++ rtabmap_demo/src/rviz/MapGraphDisplay.cpp | 182 ++++ rtabmap_demo/src/rviz/MapGraphDisplay.h | 90 ++ .../src/rviz/OrbitOrientedViewController.cpp | 76 ++ .../src/rviz/OrbitOrientedViewController.h | 51 + 46 files changed, 10182 insertions(+) create mode 100644 orbslam2_demo/CMakeLists.txt create mode 100644 orbslam2_demo/README.md create mode 100644 orbslam2_demo/launch/ros_orbslam2.launch create mode 100644 orbslam2_demo/launch/ros_orbslam2.launch~ create mode 100644 orbslam2_demo/package.xml create mode 100644 orbslam2_demo/param/Zdzn.yaml create mode 100644 "orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" create mode 100644 rtabmap_demo/CMakeLists.txt create mode 100644 rtabmap_demo/README.md create mode 100644 rtabmap_demo/launch/config/appearance_gui.ini create mode 100644 rtabmap_demo/launch/config/demo_find_object.rviz create mode 100644 rtabmap_demo/launch/config/demo_stereo_outdoor.rviz create mode 100644 rtabmap_demo/launch/config/find_object.ini create mode 100644 rtabmap_demo/launch/config/rgbd.rviz create mode 100644 rtabmap_demo/launch/config/rgbd_gui.ini create mode 100644 rtabmap_demo/launch/config/rgbdslam_datasets.rviz create mode 100644 rtabmap_demo/launch/config/rtabmap_robot_mapping.rviz create mode 100644 rtabmap_demo/launch/config/test_odometry.rviz create mode 100644 rtabmap_demo/launch/config/turtlebot_navigation.rviz create mode 100644 rtabmap_demo/launch/rtabmap_robot_mapping.launch create mode 100644 rtabmap_demo/package.xml create mode 100644 "rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" create mode 100644 rtabmap_demo/src/nodelets/data_odom_sync.cpp create mode 100644 rtabmap_demo/src/nodelets/data_throttle.cpp create mode 100644 rtabmap_demo/src/nodelets/disparity_to_depth.cpp create mode 100644 rtabmap_demo/src/nodelets/icp_odometry.cpp create mode 100644 rtabmap_demo/src/nodelets/obstacles_detection.cpp create mode 100644 rtabmap_demo/src/nodelets/obstacles_detection_old.cpp create mode 100644 rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp create mode 100644 rtabmap_demo/src/nodelets/point_cloud_xyz.cpp create mode 100644 rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp create mode 100644 rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp create mode 100644 rtabmap_demo/src/nodelets/rgbd_odometry.cpp create mode 100644 rtabmap_demo/src/nodelets/rgbd_sync.cpp create mode 100644 rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp create mode 100644 rtabmap_demo/src/nodelets/stereo_odometry.cpp create mode 100644 rtabmap_demo/src/nodelets/stereo_throttle.cpp create mode 100644 rtabmap_demo/src/nodelets/undistort_depth.cpp create mode 100644 rtabmap_demo/src/rviz/InfoDisplay.cpp create mode 100644 rtabmap_demo/src/rviz/InfoDisplay.h create mode 100644 rtabmap_demo/src/rviz/MapCloudDisplay.cpp create mode 100644 rtabmap_demo/src/rviz/MapCloudDisplay.h create mode 100644 rtabmap_demo/src/rviz/MapGraphDisplay.cpp create mode 100644 rtabmap_demo/src/rviz/MapGraphDisplay.h create mode 100644 rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp create mode 100644 rtabmap_demo/src/rviz/OrbitOrientedViewController.h diff --git a/orbslam2_demo/CMakeLists.txt b/orbslam2_demo/CMakeLists.txt new file mode 100644 index 0000000..1280fe8 --- /dev/null +++ b/orbslam2_demo/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(orbslam2_demo) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES orbslam2_demo +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/orbslam2_demo.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/orbslam2_demo_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_orbslam2_demo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/orbslam2_demo/README.md b/orbslam2_demo/README.md new file mode 100644 index 0000000..a77eac5 --- /dev/null +++ b/orbslam2_demo/README.md @@ -0,0 +1,8 @@ +# 运行ORB_SLAM2 +1. 首先应该安装openCV,注意测试版本,V2.4到V3.2.openCV的安装有些繁琐,需要安装各种依赖,否则会出现问题.建议按照参考教程完成安装并测试. +2. 安装Pangolin,需要进行初始化编译. +3. 下载orb_slam2源码包,官网github上源码包含了DBoW2,和g2o.另需要下载Eigen3.1 +4. 对orb_slam2进行编译构建. +5. 输入命令进行仿真环境下的构图，首先启动robot_sim_demo,再启动orbslam2_demo,最后启动键盘控制． + +**Tips:**　具体步骤参考－＞运行包详细步骤.md \ No newline at end of file diff --git a/orbslam2_demo/launch/ros_orbslam2.launch b/orbslam2_demo/launch/ros_orbslam2.launch new file mode 100644 index 0000000..5643c75 --- /dev/null +++ b/orbslam2_demo/launch/ros_orbslam2.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/orbslam2_demo/launch/ros_orbslam2.launch~ b/orbslam2_demo/launch/ros_orbslam2.launch~ new file mode 100644 index 0000000..fba51c2 --- /dev/null +++ b/orbslam2_demo/launch/ros_orbslam2.launch~ @@ -0,0 +1,6 @@ + + + + + + diff --git a/orbslam2_demo/package.xml b/orbslam2_demo/package.xml new file mode 100644 index 0000000..7103dca --- /dev/null +++ b/orbslam2_demo/package.xml @@ -0,0 +1,68 @@ + + + orbslam2_demo + 0.0.0 + The orbslam2_demo package + + + + + acx + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/orbslam2_demo/param/Zdzn.yaml b/orbslam2_demo/param/Zdzn.yaml new file mode 100644 index 0000000..d03c078 --- /dev/null +++ b/orbslam2_demo/param/Zdzn.yaml @@ -0,0 +1,69 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 277.19135641132203 +Camera.fy: 277.19135641132203 +Camera.cx: 160.5 +Camera.cy: 120.5 + +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.width: 320 +Camera.height: 240 + +# Camera frames per second +Camera.fps: 50.0 + +# IR projector baseline times fx (aprox.) +Camera.bf: 40.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +ThDepth: 40.0 + +# Deptmap values factor +DepthMapFactor: 1.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 + diff --git "a/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" "b/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" new file mode 100644 index 0000000..094f476 --- /dev/null +++ "b/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" @@ -0,0 +1,93 @@ +# orb_slam2在本课程仿真环境中运行 +### 详细步骤 +1. 建立ＲＯＳ工作空间（前几节课已经建好的catkin_ws/可直接使用跳过此步骤）代码如下： + + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/ + catkin_make + source devel/setup.bash + echo "source devel/setup.bash" >> ~/.bashrc + source ~/.bashrc + + source /opt/ros/kinetic/setup.bash + source ~/catkin_ws/devel/setup.bash + +2. 安装opencv 3.2（不必安装在ＲＯＳ空间)， + + * 下载两个安装包 + + [(http://opencv.org/releases.html)]( http://opencv.org/releases.html) + [(opencv/releases/tag/3.2.0 +ippicv_linux_20151201.tgz 地址)](https://raw.githubusercontent.com/opencv/opencv_3rdparty/81a676001ca8075ada498583e4166079e5744668/ippicv/ippicv_linux_20151201.tgz) + * 安装依赖库 + + $ sudo apt-get install gcc g++ cmake pkg-config build-essential + + $ sudo apt-get install libgtk2.0-dev libavcodec-dev libavformat-dev libtiff4-dev libswscale-dev libjasper-dev + + * 编译安装过程 + + $ unzip opencv-3.2.0.zip + + $ cd opencv-3.2.0 + + $ mkdir build + + $ cd build + + $ make + + $ sudo make install + * 配置pkg-config + + $ sudo vim /etc/ld.so.conf + + $ sudo ldconfig -v + + $ export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig + + 使用以下命令查看pkg-config配置信息： pkg-config opencv --libs --cflags + * 测试openCV,采用openCV自带的sample + + $ cd opencv-3.2.0/samples/ + + $ cmake . + + $ make + + $ cd cpp/ + + $ ./cpp-example-example + +3. 安装Pangolin + git clone https://github.com/stevenlovegrove/Pangolin.git + cd Pangolin + mkdir build + cd build + cmake -DCPP11_NO_BOOST=1 .. + make -j + +4. 下载orb_salm2的源码包 + + 在ＲＯＳ的工作空间下　`catkin_ws/` 　输入命令： +`git clone https://github.com/raulmur/ORB_SLAM2.git` + +5. 构建orb_slam2 + + cd ORB_SLAM2 + chmod +x build.sh + ./build.sh + chmod +x build_ros.sh + ./build_ros.sh + + cd + grdit .bashrc + 在末尾加上：export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/【本机】/catkin_ws/src/ORB_SLAM2/Examples/ROS +6. 执行语句开始进行仿真建图 + + + 　 终端1：roslaunch robot_sim_demo robot_spawn.launch + 　终端2：roslaunch orbslam2_demo ros_orbslam2.launch + 　 终端3：rosrun robot_sim_demo robot_keyboard_teleop.py + + \ No newline at end of file diff --git a/rtabmap_demo/CMakeLists.txt b/rtabmap_demo/CMakeLists.txt new file mode 100644 index 0000000..ba3bfba --- /dev/null +++ b/rtabmap_demo/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rtabmap_demo) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES rtabmap_demo +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/rtabmap_demo.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/rtabmap_demo_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rtabmap_demo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rtabmap_demo/README.md b/rtabmap_demo/README.md new file mode 100644 index 0000000..1e74836 --- /dev/null +++ b/rtabmap_demo/README.md @@ -0,0 +1,10 @@ +# rtabmap 在仿真环境下的运行 + +### 操作步骤 +1. 根据自己的ROS版本,安装对应的rtabmap-ros +2. 从源构建,添加依赖(包括可选的OpenCV,g2o,GTSAM,Freenect2) +3. 安装RTAM-Map(注意不要直接克隆在catkin 工作空间下) +4. 更新版本 +5. 输入指令,运行环境,开始建图. + +##### 详细操作请参考:rtabmap 在仿真环境下的运行操作指南 \ No newline at end of file diff --git a/rtabmap_demo/launch/config/appearance_gui.ini b/rtabmap_demo/launch/config/appearance_gui.ini new file mode 100644 index 0000000..9e37933 --- /dev/null +++ b/rtabmap_demo/launch/config/appearance_gui.ini @@ -0,0 +1,166 @@ +[Gui] +General\imagesKept=true +General\loggerLevel=2 +General\loggerEventLevel=3 +General\loggerPauseLevel=4 +General\loggerType=1 +General\loggerPrintTime=true +General\verticalLayoutUsed=false +General\imageRejectedShown=true +General\imageHighestHypShown=true +General\beep=false +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\a\0\0\x2\x30\xfc\x2\0\0\0\x2\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x30\0\0\x2\x30\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1\xf4\0\0\0\x19\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x30\xfc\x2\0\0\0\x3\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0(\0\0\x1\xf4\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x2\x30\0\0\0+\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0O\0\xff\xff\xff\0\0\0\0\0\0\x2\x30\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc5\0\0\0K\0\0\x5\xd8\0\0\x3t\0\0\0\xcf\0\0\0q\0\0\x5\xce\0\0\x3j\0\0\0\0\0\0) +General\showClouds0=true +General\decimation0=4 +General\maxDepth0=4 +General\showScans0=true +General\opacity0=0.6 +General\ptSize0=1 +General\opacityScan0=1 +General\ptSizeScan0=1 +General\showClouds1=true +General\decimation1=2 +General\maxDepth1=0 +General\showScans1=true +General\opacity1=1 +General\ptSize1=1 +General\opacityScan1=1 +General\ptSizeScan1=1 +General\cloudFiltering=false +General\cloudFilteringRadius=0.5 +General\cloudFilteringAngle=30 +MainWindow\maximized=false +MainWindow\status_bar=false +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0) +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0) +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_focal=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0) +widget_cloudViewer\grid=false +widget_cloudViewer\grid_cell_count=50 +widget_cloudViewer\grid_cell_size=1 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\image_shown=true +imageView_source\depth_shown=false +imageView_source\features_shown=true +imageView_source\lines_shown=true +imageView_source\alpha=50 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_loopClosure\image_shown=true +imageView_loopClosure\depth_shown=false +imageView_loopClosure\features_shown=true +imageView_loopClosure\lines_shown=true +imageView_loopClosure\alpha=50 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_odometry\image_shown=true +imageView_odometry\depth_shown=false +imageView_odometry\features_shown=true +imageView_odometry\lines_shown=true +imageView_odometry\alpha=200 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +ExportCloudsDialog\binary=true +ExportCloudsDialog\normals_k=6 +ExportCloudsDialog\regenerate=false +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_radius=0.02 +ExportCloudsDialog\filtering_min_neighbors=2 +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_voxel=0.01 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_point_radius=0.02 +ExportCloudsDialog\subtract_point_angle=45 +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_radius=0.04 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0 +ExportCloudsDialog\mls_point_density=0 +ExportCloudsDialog\mls_dilation_voxel_size=0.01 +ExportCloudsDialog\mls_dilation_iterations=0 +ExportCloudsDialog\mesh=false +ExportCloudsDialog\mesh_radius=0.04 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_triangle_size=2 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\cluster_radius=0.5 +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\iterations=1 +PostProcessingDialog\reextract_features=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\refine_lc=false +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_epsilon=0.0001 +PostProcessingDialog\sba_inlier_distance=0.05 +PostProcessingDialog\sba_min_inliers=10 +graphicsView_graphView\node_radius=0.00999999977648258 +graphicsView_graphView\link_width=0 +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\grid_visible=true +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +General\loggerPrintThreadId=false +General\notifyNewGlobalPath=false +General\odomQualityThr=50 +General\posteriorGraphView=true +General\showFeatures0=false +General\downsamplingScan0=1 +General\voxelSizeScan0=0 +General\ptSizeFeatures0=3 +General\showFeatures1=true +General\downsamplingScan1=1 +General\voxelSizeScan1=0 +General\ptSizeFeatures1=3 +General\showGraphs=true +General\showLabels=false +General\noFiltering=true +General\subtractFiltering=false +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\subtractFilteringAngle=45 +General\gridMapShown=false +General\gridMapResolution=0.05 +General\gridMapOccupancyFrom3DCloud=false +General\gridMapEroded=false +General\gridMapOpacity=0.75 +General\meshing=false +General\meshing_angle=15 +General\meshing_quad=false +General\meshing_triangle_size=2 +Figures\counts=1 +Figures\curves=Loop/Highest_hypothesis_value/ diff --git a/rtabmap_demo/launch/config/demo_find_object.rviz b/rtabmap_demo/launch/config/demo_find_object.rviz new file mode 100644 index 0000000..5f6435f --- /dev/null +++ b/rtabmap_demo/launch/config/demo_find_object.rviz @@ -0,0 +1,332 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + Splitter Ratio: 0.434783 + Tree Height: 590 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.02684 + Min Value: -1.12646 + Value: true + Axis: Z + Channel Name: rgb + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2.34177e-38 + Min Color: 0; 0; 0 + Min Intensity: 9.21942e-41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /voxel_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + map: + Value: true + object_4: + Value: true + object_5: + Value: true + object_6: + Value: true + object_7: + Value: true + object_8: + Value: true + odom: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + object_4: + {} + object_5: + {} + object_6: + {} + object_7: + {} + object_8: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: -0.574923 + Min Value: -5.11672 + Value: true + Axis: X + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.01 + Style: Points + Topic: /base_scan + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /rtabmap/grid_map + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/data_throttled_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 4 + Cloud max depth (m): 3 + Cloud voxel size (m): 0.02 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 20 + Node filtering radius (m): 0.2 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rtabmap_ros/Info + Enabled: true + Name: Info + Topic: /rtabmap/info + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rtabmap_ros/OrbitOriented + Distance: 7.89956 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0199225 + Y: 0.23516 + Z: -0.0896359 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.819797 + Target Frame: base_link + Value: OrbitOriented (rtabmap) + Yaw: 3.14375 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 763 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000dd00fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1561 + X: 42 + Y: 107 diff --git a/rtabmap_demo/launch/config/demo_stereo_outdoor.rviz b/rtabmap_demo/launch/config/demo_stereo_outdoor.rviz new file mode 100644 index 0000000..a827b44 --- /dev/null +++ b/rtabmap_demo/launch/config/demo_stereo_outdoor.rviz @@ -0,0 +1,318 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 420 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + az3_base_link: + Value: true + az3_odom: + Value: true + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + stereo_camera: + Value: true + stereo_camera_base: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + stereo_camera_base: + stereo_camera: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 4 + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /rtabmap/mapData + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rtabmap_ros/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: /rtabmap/mapGraph + Unreliable: false + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Class: rviz/Image + Enabled: true + Image Topic: /stereo_camera/left/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /rtabmap/grid_map + Unreliable: false + Value: true + - Class: rtabmap_ros/Info + Enabled: true + Name: Info + Topic: /rtabmap/info + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.17744994 + Min Value: -1.72299004 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /rtabmap/odom_local_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomFrame + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /rtabmap/odom_last_frame + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rtabmap_ros/OrbitOriented + Distance: 8.28384018 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.109544002 + Y: 0.145557001 + Z: 0.113348998 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.635397971 + Target Frame: base_footprint + Value: OrbitOriented (rtabmap) + Yaw: 3.0704 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 848 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000030afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000233000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000261000000d10000001600ffffff000000010000010f000002edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002ed000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c40000003efc0100000002fb0000000800540069006d00650000000000000004c40000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003cd0000030a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1341 + X: 97 + Y: 24 diff --git a/rtabmap_demo/launch/config/find_object.ini b/rtabmap_demo/launch/config/find_object.ini new file mode 100644 index 0000000..1c25157 --- /dev/null +++ b/rtabmap_demo/launch/config/find_object.ini @@ -0,0 +1,136 @@ +[General] +windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\x2\0\0\x2\xeb\0\0\x2\x9f\0\0\x4\x32\0\0\x1\n\0\0\x3\a\0\0\x2\x97\0\0\x4*\0\0\0\0\0\0) +windowState=@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xc8\0\0\x3\a\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\x8a\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x3\a\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x19\0\0\x3\a\0\0\0\x9c\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\b\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x8e\0\0\x1\xe\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0) + +[Camera] +1deviceId=0 +2imageWidth=640 +3imageHeight=480 +4imageRate=0 +5mediaPath= +6useTcpCamera=false +7IP=127.0.0.1 +8port=5000 +9queueSize=1 + +[Feature2D] +1Detector="5:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK" +2Descriptor="2:Brief;ORB;SIFT;SURF;BRISK;FREAK" +3MaxFeatures=0 +BRISK_octaves=3 +BRISK_patternScale=1 +BRISK_thresh=30 +Brief_bytes=32 +Dense_featureScaleLevels=1 +Dense_featureScaleMul=0.1 +Dense_initFeatureScale=1 +Dense_initImgBound=0 +Dense_initXyStep=6 +Dense_varyImgBoundWithScale=false +Dense_varyXyStepWithScale=true +FREAK_nOctaves=4 +FREAK_orientationNormalized=true +FREAK_patternScale=22 +FREAK_scaleNormalized=true +Fast_gpu=false +Fast_keypointsRatio=0.05 +Fast_nonmaxSuppression=true +Fast_threshold=10 +GFTT_blockSize=3 +GFTT_k=0.04 +GFTT_maxCorners=1000 +GFTT_minDistance=1 +GFTT_qualityLevel=0.01 +GFTT_useHarrisDetector=false +MSER_areaThreshold=1.01 +MSER_delta=5 +MSER_edgeBlurSize=5 +MSER_maxArea=14400 +MSER_maxEvolution=200 +MSER_maxVariation=0.25 +MSER_minArea=60 +MSER_minDiversity=0.2 +MSER_minMargin=0.003 +ORB_WTA_K=2 +ORB_edgeThreshold=31 +ORB_firstLevel=0 +ORB_gpu=false +ORB_nFeatures=500 +ORB_nLevels=8 +ORB_patchSize=31 +ORB_scaleFactor=1.2 +ORB_scoreType=0 +SIFT_contrastThreshold=0.04 +SIFT_edgeThreshold=10 +SIFT_nOctaveLayers=3 +SIFT_nfeatures=0 +SIFT_sigma=1.6 +SURF_extended=true +SURF_gpu=false +SURF_hessianThreshold=600 +SURF_keypointsRatio=0.01 +SURF_nOctaveLayers=2 +SURF_nOctaves=4 +SURF_upright=false +Star_lineThresholdBinarized=8 +Star_lineThresholdProjected=10 +Star_maxSize=45 +Star_responseThreshold=30 +Star_suppressNonmaxSize=5 + +[%General] +autoScroll=true +autoStartCamera=false +autoUpdateObjects=true +controlsShown=false +imageFormats=*.png *.jpg *.bmp *.tiff *.ppm +invertedSearch=true +mirrorView=false +multiDetection=false +multiDetectionRadius=30 +nextObjID=9 +port=0 +sendNoObjDetectedEvents=false +threads=1 +videoFormats=*.avi *.m4v *.mp4 +vocabularyIncremental=false +vocabularyUpdateMinWords=2000 +autoPauseOnDetection=false + +[Homography] +homographyComputed=true +ignoreWhenAllInliers=false +method="1:LMEDS;RANSAC" +minimumInliers=10 +ransacReprojThr=5 +rectBorderWidth=4 +allCornersVisible=false +minAngle=50 + +[NearestNeighbor] +1Strategy="1:Linear;KDTree;KMeans;Composite;Autotuned;Lsh" +2Distance_type="0:EUCLIDEAN_L2;MANHATTAN_L1;MINKOWSKI;MAX;HIST_INTERSECT;HELLINGER;CHI_SQUARE_CS;KULLBACK_LEIBLER_KL;HAMMING" +3nndrRatioUsed=true +4nndrRatio=0.8 +5minDistanceUsed=false +6minDistance=1.6 +7search_checks=32 +8search_eps=0 +9search_sorted=true +Autotuned_build_weight=0.01 +Autotuned_memory_weight=0 +Autotuned_sample_fraction=0.1 +Autotuned_target_precision=0.8 +Composite_branching=32 +Composite_cb_index=0.2 +Composite_centers_init="0:RANDOM;GONZALES;KMEANSPP" +Composite_iterations=11 +Composite_trees=4 +KDTree_trees=4 +KMeans_branching=32 +KMeans_cb_index=0.2 +KMeans_centers_init="0:RANDOM;GONZALES;KMEANSPP" +KMeans_iterations=11 +Lsh_key_size=20 +Lsh_multi_probe_level=2 +Lsh_table_number=12 diff --git a/rtabmap_demo/launch/config/rgbd.rviz b/rtabmap_demo/launch/config/rgbd.rviz new file mode 100644 index 0000000..e9c36c2 --- /dev/null +++ b/rtabmap_demo/launch/config/rgbd.rviz @@ -0,0 +1,208 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /Info1 + Splitter Ratio: 0.411058009 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.02683997 + Min Value: -1.12645996 + Value: true + Axis: Z + Channel Name: rgb + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2.34176998e-38 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /voxel_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/rgb/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 4 + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.00999999978 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /rtabmap/mapData + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rtabmap_ros/Info + Enabled: true + Name: Info + Topic: /rtabmap/info + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.35253 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.14572001 + Y: 0.168097004 + Z: -1.13136005 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.879796982 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 3.73417997 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 824 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003890000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1273 + X: 86 + Y: 48 diff --git a/rtabmap_demo/launch/config/rgbd_gui.ini b/rtabmap_demo/launch/config/rgbd_gui.ini new file mode 100644 index 0000000..745754b --- /dev/null +++ b/rtabmap_demo/launch/config/rgbd_gui.ini @@ -0,0 +1,204 @@ +[Gui] +General\imagesKept=true +General\loggerLevel=2 +General\loggerEventLevel=3 +General\loggerPauseLevel=3 +General\loggerType=1 +General\loggerPrintTime=true +General\verticalLayoutUsed=true +General\imageRejectedShown=true +General\imageHighestHypShown=true +General\beep=false +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\xa8\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\xa8\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0O\0\xff\xff\xff\xfc\x2\0\0\0\x2\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\0(\0\0\x1%\0\0\0\x19\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1S\0\0\x1}\0\0\0=\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0m\0\xff\xff\xff\0\0\0\0\0\0\x2\xa8\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc4\0\0\0\xc4\0\0\x5\xd7\0\0\x3\xc3\0\0\0\xce\0\0\0\xea\0\0\x5\xcd\0\0\x3\xb9\0\0\0\0\0\0) +General\showClouds0=true +General\decimation0=4 +General\maxDepth0=4 +General\showScans0=true +General\opacity0=1 +General\ptSize0=1 +General\opacityScan0=1 +General\ptSizeScan0=1 +General\showClouds1=true +General\decimation1=4 +General\maxDepth1=0 +General\showScans1=true +General\opacity1=1 +General\ptSize1=2 +General\opacityScan1=1 +General\ptSizeScan1=1 +General\cloudFiltering=false +General\cloudFilteringRadius=0.2 +General\cloudFilteringAngle=20 +General\odomQualityThr=50 +General\meshing=false +General\gridMapShown=false +General\gridMapResolution=0.05 +General\gridMapOccupancyFrom3DCloud=false +General\gridMapOpacity=0.75 +General\posteriorGraphView=true +General\showGraphs=true +General\octomap=false +General\octomap_depth=16 +General\octomap_ground_is_obstacle=true + +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\0\0\0\0) +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0) +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\x13\0\0\0\0\xbe\xb6\0\0\0\0\0\0>\xa4\0\0\0\0\0\0) +widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>\xa4\0\0\0\0\0\0) +widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0) +widget_cloudViewer\grid=false +widget_cloudViewer\grid_cell_count=50 +widget_cloudViewer\grid_cell_size=1 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\image_shown=true +imageView_source\depth_shown=false +imageView_source\features_shown=true +imageView_source\lines_shown=true +imageView_loopClosure\image_shown=true +imageView_loopClosure\depth_shown=false +imageView_loopClosure\features_shown=true +imageView_loopClosure\lines_shown=true +imageView_odometry\image_shown=true +imageView_odometry\depth_shown=false +imageView_odometry\features_shown=true +imageView_odometry\lines_shown=true +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_voxel=0.005 +ExportCloudsDialog\regenerate=true +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_voxel=0.005 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\binary=true +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_radius=0.04 +ExportCloudsDialog\mesh=false +ExportCloudsDialog\mesh_k=20 +ExportCloudsDialog\mesh_radius=0.04 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\cluster_radius=0.3 +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\iterations=1 +PostProcessingDialog\reextract_features=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\refine_lc=false +MainWindow\maximized=false +MainWindow\status_bar=false +widget_cloudViewer\frustum_shown=false +widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0) +widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +widget_cloudViewer\rendering_rate=5 +imageView_source\alpha=50 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_loopClosure\alpha=50 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_odometry\alpha=200 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +ExportCloudsDialog\pipeline=0 +ExportCloudsDialog\normals_k=10 +ExportCloudsDialog\regenerate_min_depth=0 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_radius=0.02 +ExportCloudsDialog\filtering_min_neighbors=2 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_point_radius=0.02 +ExportCloudsDialog\subtract_point_angle=0 +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0 +ExportCloudsDialog\mls_point_density=0 +ExportCloudsDialog\mls_dilation_voxel_size=0.01 +ExportCloudsDialog\mls_dilation_iterations=0 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_triangle_size=2 +ExportScansDialog\binary=true +ExportScansDialog\normals_k=20 +ExportScansDialog\regenerate=false +ExportScansDialog\regenerate_decimation=1 +ExportScansDialog\filtering=false +ExportScansDialog\filtering_radius=0.02 +ExportScansDialog\filtering_min_neighbors=2 +ExportScansDialog\assemble=true +ExportScansDialog\assemble_voxel=0.01 +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_epsilon=0 +PostProcessingDialog\sba_type=1 +PostProcessingDialog\sba_variance=1 +graphicsView_graphView\node_radius=0.00999999977648258 +graphicsView_graphView\link_width=0 +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\grid_visible=true +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +graphicsView_graphView\graph_visible=true +graphicsView_graphView\global_path_visible=true +graphicsView_graphView\local_path_visible=true +graphicsView_graphView\gt_graph_visible=true +General\cloudsKept=true +General\loggerPrintThreadId=false +General\notifyNewGlobalPath=false +General\minDepth0=0 +General\showFeatures0=false +General\downsamplingScan0=1 +General\voxelSizeScan0=0 +General\ptSizeFeatures0=3 +General\minDepth1=0 +General\showFeatures1=true +General\downsamplingScan1=1 +General\voxelSizeScan1=0 +General\ptSizeFeatures1=3 +General\cloudVoxel=0 +General\cloudNoiseRadius=0 +General\cloudNoiseMinNeighbors=5 +General\showLabels=false +General\noFiltering=true +General\subtractFiltering=false +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\subtractFilteringAngle=0 +General\normalKSearch=10 +General\gridMapEroded=false +General\projMapFrame=true +General\projMaxGroundAngle=30 +General\projMaxGroundHeight=0 +General\projMinClusterSize=20 +General\projMaxObstaclesHeight=0 +General\projFlatObstaclesDetected=true +General\meshing_angle=15 +General\meshing_quad=false +General\meshing_triangle_size=2 +Figures\counts= +Figures\curves= \ No newline at end of file diff --git a/rtabmap_demo/launch/config/rgbdslam_datasets.rviz b/rtabmap_demo/launch/config/rgbdslam_datasets.rviz new file mode 100644 index 0000000..8b361ef --- /dev/null +++ b/rtabmap_demo/launch/config/rgbdslam_datasets.rviz @@ -0,0 +1,291 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Info1 + - /PointCloud23 + - /PointCloud23/Status1 + Splitter Ratio: 0.529595 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.02684 + Min Value: -1.12646 + Value: true + Axis: Z + Channel Name: rgb + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2.34177e-38 + Min Color: 0; 0; 0 + Min Intensity: 9.21942e-41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /voxel_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + kinect: + Value: true + kinect_est: + Value: true + map: + Value: true + odom: + Value: true + openni_camera: + Value: true + openni_depth_frame: + Value: true + openni_depth_optical_frame: + Value: true + openni_rgb_frame: + Value: true + openni_rgb_optical_frame: + Value: true + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + kinect: + openni_camera: + openni_depth_frame: + openni_depth_optical_frame: + {} + openni_rgb_frame: + openni_rgb_optical_frame: + {} + map: + odom: + kinect_est: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/rgb/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 4 + Cloud max depth (m): 4 + Cloud voxel size (m): 0.01 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 15 + Node filtering radius (m): 0.1 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rtabmap_ros/Info + Enabled: true + Name: Info + Topic: /rtabmap/info + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.17984 + Min Value: 0.0147058 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 255; 127 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.32013 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.216951 + Y: 0.819032 + Z: 1.30659 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.554801 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 4.9892 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 824 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1273 + X: 316 + Y: 95 diff --git a/rtabmap_demo/launch/config/rtabmap_robot_mapping.rviz b/rtabmap_demo/launch/config/rtabmap_robot_mapping.rviz new file mode 100644 index 0000000..5e44516 --- /dev/null +++ b/rtabmap_demo/launch/config/rtabmap_robot_mapping.rviz @@ -0,0 +1,306 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /MapGraph1 + Splitter Ratio: 0.411058009 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.02683997 + Min Value: -1.12645996 + Value: true + Axis: Z + Channel Name: rgb + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2.34176998e-38 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /voxel_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 8.34969044 + Min Value: -0.777028978 + Value: true + Axis: X + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /jn0/base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + camera_link: + Value: true + camera_link_optical: + Value: true + imu_link: + Value: true + laser: + Value: true + laser_mount_link: + Value: true + left_wheel: + Value: true + map: + Value: true + odom: + Value: true + pitch_platform: + Value: true + right_wheel: + Value: true + yaw_platform: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + imu_link: + {} + laser_mount_link: + laser: + {} + yaw_platform: + pitch_platform: + camera_link: + camera_link_optical: + {} + left_wheel: + {} + right_wheel: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/rgb/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 8 + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.00999999978 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /mapData + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rtabmap_ros/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: /rtabmap/mapGraph + Unreliable: false + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /proj_map + Unreliable: false + Use Timestamp: false + Value: true + - Class: rtabmap_ros/Info + Enabled: true + Name: Info + Topic: /rtabmap/info + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.93953991 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0336608998 + Y: 0.112554997 + Z: -0.719693005 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.369796425 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.5791719 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 824 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000d700fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002ff00fffffffb0000000800540069006d00650100000000000004500000000000000000000003890000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1273 + X: 203 + Y: 46 diff --git a/rtabmap_demo/launch/config/test_odometry.rviz b/rtabmap_demo/launch/config/test_odometry.rviz new file mode 100644 index 0000000..5684d64 --- /dev/null +++ b/rtabmap_demo/launch/config/test_odometry.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + Splitter Ratio: 0.411058 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: odom + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.02684 + Min Value: -1.12646 + Value: true + Axis: Z + Channel Name: rgb + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2.34177e-38 + Min Color: 0; 0; 0 + Min Intensity: 9.21942e-41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /voxel_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + base_link: + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/rgb/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.01 + Style: Points + Topic: /odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.06539 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.45837 + Y: 0.109967 + Z: -0.602093 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.839796 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 3.58419 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 824 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1273 + X: 229 + Y: 149 diff --git a/rtabmap_demo/launch/config/turtlebot_navigation.rviz b/rtabmap_demo/launch/config/turtlebot_navigation.rviz new file mode 100644 index 0000000..4dff18b --- /dev/null +++ b/rtabmap_demo/launch/config/turtlebot_navigation.rviz @@ -0,0 +1,446 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Map1/Costmap1 + - /Global Map1/Planner1 + - /Local Map1/Costmap1 + - /Local Map1/Planner1 + Splitter Ratio: 0.5 + Tree Height: 481 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Rtabmap cloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cliff_sensor_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_sensor_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_sensor_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + plate_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + plate_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + plate_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_kinect_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_kinect_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Squares + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.08 + Style: Spheres + Topic: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + - Class: rviz/Group + Displays: + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Costmap + Topic: /move_base/global_costmap/costmap + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Name: Planner + Topic: /move_base/TrajectoryPlannerROS/global_plan + Value: true + Enabled: true + Name: Global Map + - Class: rviz/Group + Displays: + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Costmap + Topic: /move_base/local_costmap/costmap + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 12; 255 + Enabled: true + Name: Planner + Topic: /move_base/TrajectoryPlannerROS/local_plan + Value: true + Enabled: true + Name: Local Map + - Arrow Length: 0.2 + Class: rviz/PoseArray + Color: 0; 192; 0 + Enabled: true + Name: Amcl Particles + Topic: /particlecloud + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_ros/MapCloud + Cloud decimation: 4 + Cloud max depth (m): 4 + Cloud voxel size (m): 0.05 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Enabled: true + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Rtabmap cloud + Node filtering angle (degrees): 0 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/Measure + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.31174 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.00901246 + Y: -0.126793 + Z: 0.0223136 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.3598 + Target Frame: base_footprint + Value: Orbit (rviz) + Yaw: 3.12914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 694 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e500000270fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000270000000dd00ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000270000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002df0000027000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1503 + X: 85 + Y: 188 diff --git a/rtabmap_demo/launch/rtabmap_robot_mapping.launch b/rtabmap_demo/launch/rtabmap_robot_mapping.launch new file mode 100644 index 0000000..8228464 --- /dev/null +++ b/rtabmap_demo/launch/rtabmap_robot_mapping.launch @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap_demo/package.xml b/rtabmap_demo/package.xml new file mode 100644 index 0000000..e1515aa --- /dev/null +++ b/rtabmap_demo/package.xml @@ -0,0 +1,68 @@ + + + rtabmap_demo + 0.0.0 + The rtabmap_demo package + + + + + acx + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" new file mode 100644 index 0000000..1902bfd --- /dev/null +++ "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" @@ -0,0 +1,74 @@ +# rtabmap在仿真环境下的运行操作指南 +### 操作过程 +1. 根据自己的ROS版本,安装对应的rtabmap-ros + * lunar + + $ sudo apt-get install ros-lunar-rtabmap-ros + * Kinetic + $ sudo apt-get install ros-kinetic-rtabmap-ros + + * Jade + + $ sudo apt-get install ros-jade-rtabmap-ros + * Indigo + $ sudo apt-get install ros-indigo-rtabmap-ros + + * Hydro + + $ sudo apt-get install ros-hydro-rtabmap-ros + +2. 从源构建,添加依赖 + + * 首先检查 `~/.bashrc`文件是否包含下面两句: + + $ source /opt/ros/kinetic/setup.bash + $ source ~/catkin_ws/devel/setup.bash + * 要求安装的依赖: + + $ sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros + $ sudo apt-get remove ros-kinetic-rtabmap ros-kinetic-rtabmap-ros + + * 可选择安装的依赖 + + 包括OpenCV,不同的版本依赖不同,g2o,GTSAM,Freenect2.可根据自己的情况选择安装.具体可参考访问[here!](https://github.com/introlab/rtabmap_ros#rtabmap_ros) +3. 安装RTAM-Map(注意不要直接克隆在catkin 工作空间下) + + $ cd ~ + $ git clone https://github.com/introlab/rtabmap.git rtabmap + $ cd rtabmap/build + $ cmake .. [<---两个点] + $ make + $ sudo make install + + $ cd ~/catkin_ws + $ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros + $ catkin_make -j1 +4. 更新版本 + + $ cd rtabmap/build + $ make uninstall + $ sudo make uninstall + + $ cd .. + $ rm -rf build/* + $ git pull origin master + $ cd build + + $ cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel .. + $ cmake .. + + $ make + $ make install + + $ roscd rtabmap_ros + $ git pull origin master + $ cd ~/catkin_ws + $ rm -rf build/CMakeCache.txt build/rtabmap_ros + $ catkin_make -j1 --pkg rtabmap_ros + +5. 输入指令,开始仿真建图 + + 终端1:$ roslaunch robot_sim_demo robot_spawn.launch #启动仿真环境 + 终端2:$ roslaunch rtabmap_demo rtabmap_robot_mapping.launch #启动rtabmap + 终端3:$ rosrun robot_sim_demo robot_keyboard_teleop.py #启动键盘控制 + \ No newline at end of file diff --git a/rtabmap_demo/src/nodelets/data_odom_sync.cpp b/rtabmap_demo/src/nodelets/data_odom_sync.cpp new file mode 100644 index 0000000..6725af9 --- /dev/null +++ b/rtabmap_demo/src/nodelets/data_odom_sync.cpp @@ -0,0 +1,130 @@ +/* +Copyright (c) 2010-2016, 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 "ros/ros.h" +#include "pluginlib/class_list_macros.h" +#include "nodelet/nodelet.h" + +#include +#include +#include + +#include +#include + +#include +#include + +namespace rtabmap_ros +{ + +class DataOdomSyncNodelet : public nodelet::Nodelet +{ +public: + //Constructor + DataOdomSyncNodelet(): + sync_(0) + { + } + + virtual ~DataOdomSyncNodelet() + { + delete sync_; + } + +private: + virtual void onInit() + { + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(private_nh, "rgb"); + ros::NodeHandle depth_pnh(private_nh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + int queueSize = 10; + private_nh.param("queue_size", queueSize, queueSize); + + sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_); + sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, _1, _2, _3, _4)); + + image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info_in", 1); + odom_sub_.subscribe(nh, "odom_in", 1); + + imagePub_ = rgb_it.advertise("image_out", 1); + imageDepthPub_ = depth_it.advertise("image_out", 1); + infoPub_ = rgb_nh.advertise("camera_info_out", 1); + odomPub_ = nh.advertise("odom_out", 1); + }; + + void callback(const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& imageDepth, + const sensor_msgs::CameraInfoConstPtr& camInfo, + const nav_msgs::OdometryConstPtr & odom) + { + if(imagePub_.getNumSubscribers()) + { + imagePub_.publish(image); + } + if(imageDepthPub_.getNumSubscribers()) + { + imageDepthPub_.publish(imageDepth); + } + if(infoPub_.getNumSubscribers()) + { + infoPub_.publish(camInfo); + } + if(odomPub_.getNumSubscribers()) + { + odomPub_.publish(odom); + } + } + + image_transport::Publisher imagePub_; + image_transport::Publisher imageDepthPub_; + ros::Publisher infoPub_; + ros::Publisher odomPub_; + + image_transport::SubscriberFilter image_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + message_filters::Subscriber odom_sub_; + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + message_filters::Synchronizer * sync_; + +}; + + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataOdomSyncNodelet, nodelet::Nodelet); +} diff --git a/rtabmap_demo/src/nodelets/data_throttle.cpp b/rtabmap_demo/src/nodelets/data_throttle.cpp new file mode 100644 index 0000000..47d266b --- /dev/null +++ b/rtabmap_demo/src/nodelets/data_throttle.cpp @@ -0,0 +1,220 @@ +/* +Copyright (c) 2010-2016, 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 "ros/ros.h" +#include "pluginlib/class_list_macros.h" +#include "nodelet/nodelet.h" + +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include + +namespace rtabmap_ros +{ + +class DataThrottleNodelet : public nodelet::Nodelet +{ +public: + //Constructor + DataThrottleNodelet(): + rate_(0), + approxSync_(0), + exactSync_(0), + decimation_(1) + { + } + + virtual ~DataThrottleNodelet() + { + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + } + +private: + ros::Time last_update_; + double rate_; + virtual void onInit() + { + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& private_nh = getPrivateNodeHandle(); + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(private_nh, "rgb"); + ros::NodeHandle depth_pnh(private_nh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + int queueSize = 10; + bool approxSync = true; + if(private_nh.getParam("max_rate", rate_)) + { + NODELET_WARN("\"max_rate\" is now known as \"rate\"."); + } + private_nh.param("rate", rate_, rate_); + private_nh.param("queue_size", queueSize, queueSize); + private_nh.param("approx_sync", approxSync, approxSync); + private_nh.param("decimation", decimation_, decimation_); + ROS_ASSERT(decimation_ >= 1); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_); + approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_); + exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3)); + } + + image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info_in", 1); + + imagePub_ = rgb_it.advertise("image_out", 1); + imageDepthPub_ = depth_it.advertise("image_out", 1); + infoPub_ = rgb_nh.advertise("camera_info_out", 1); + }; + + void callback(const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& imageDepth, + const sensor_msgs::CameraInfoConstPtr& camInfo) + { + if (rate_ > 0.0) + { + NODELET_DEBUG("update set to %f", rate_); + if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) + { + NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); + return; + } + } + else + NODELET_DEBUG("rate unset continuing"); + + last_update_ = ros::Time::now(); + + if(imagePub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); + cv_bridge::CvImage out; + out.header = imagePtr->header; + out.encoding = imagePtr->encoding; + out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); + imagePub_.publish(out.toImageMsg()); + } + else + { + imagePub_.publish(image); + } + } + if(imageDepthPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth); + cv_bridge::CvImage out; + out.header = imagePtr->header; + out.encoding = imagePtr->encoding; + out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); + imageDepthPub_.publish(out.toImageMsg()); + } + else + { + imageDepthPub_.publish(imageDepth); + } + } + if(infoPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + sensor_msgs::CameraInfo info = *camInfo; + info.height /= decimation_; + info.width /= decimation_; + info.roi.height /= decimation_; + info.roi.width /= decimation_; + info.K[2]/=float(decimation_); // cx + info.K[5]/=float(decimation_); // cy + info.K[0]/=float(decimation_); // fx + info.K[4]/=float(decimation_); // fy + info.P[2]/=float(decimation_); // cx + info.P[6]/=float(decimation_); // cy + info.P[0]/=float(decimation_); // fx + info.P[5]/=float(decimation_); // fy + infoPub_.publish(info); + } + else + { + infoPub_.publish(camInfo); + } + } + } + + image_transport::Publisher imagePub_; + image_transport::Publisher imageDepthPub_; + ros::Publisher infoPub_; + + image_transport::SubscriberFilter image_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + + int decimation_; + +}; + + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataThrottleNodelet, nodelet::Nodelet); +} diff --git a/rtabmap_demo/src/nodelets/disparity_to_depth.cpp b/rtabmap_demo/src/nodelets/disparity_to_depth.cpp new file mode 100644 index 0000000..3c5628c --- /dev/null +++ b/rtabmap_demo/src/nodelets/disparity_to_depth.cpp @@ -0,0 +1,140 @@ +/* +Copyright (c) 2010-2016, 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 + +namespace rtabmap_ros +{ + +class DisparityToDepth : public nodelet::Nodelet +{ +public: + DisparityToDepth() {} + + virtual ~DisparityToDepth(){} + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + image_transport::ImageTransport it(nh); + pub32f_ = it.advertise("depth", 1); + pub16u_ = it.advertise("depth_raw", 1); + sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this); + } + + void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg) + { + if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) + { + NODELET_ERROR("Input type must be disparity=32FC1"); + return; + } + + bool publish32f = pub32f_.getNumSubscribers(); + bool publish16u = pub16u_.getNumSubscribers(); + + if(publish32f || publish16u) + { + // sensor_msgs::image_encodings::TYPE_32FC1 + cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); + + cv::Mat depth32f; + cv::Mat depth16u; + if(publish32f) + { + depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F); + } + if(publish16u) + { + depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U); + } + for (int i = 0; i < disparity.rows; i++) + { + for (int j = 0; j < disparity.cols; j++) + { + float disparity_value = disparity.at(i,j); + if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity) + { + // baseline * focal / disparity + float depth = disparityMsg->T * disparityMsg->f / disparity_value; + if(publish32f) + { + depth32f.at(i,j) = depth; + } + if(publish16u) + { + depth16u.at(i,j) = (unsigned short)(depth*1000.0f); + } + } + } + } + + if(publish32f) + { + // convert to ROS sensor_msg::Image + cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f); + sensor_msgs::Image depthMsg; + cvDepth.toImageMsg(depthMsg); + + //publish the message + pub32f_.publish(depthMsg); + } + + if(publish16u) + { + // convert to ROS sensor_msg::Image + cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u); + sensor_msgs::Image depthMsg; + cvDepth.toImageMsg(depthMsg); + + //publish the message + pub16u_.publish(depthMsg); + } + } +} + +private: + image_transport::Publisher pub32f_; + image_transport::Publisher pub16u_; + ros::Subscriber sub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DisparityToDepth, nodelet::Nodelet); +} diff --git a/rtabmap_demo/src/nodelets/icp_odometry.cpp b/rtabmap_demo/src/nodelets/icp_odometry.cpp new file mode 100644 index 0000000..dda73e0 --- /dev/null +++ b/rtabmap_demo/src/nodelets/icp_odometry.cpp @@ -0,0 +1,347 @@ +/* +Copyright (c) 2010-2016, 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 "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class ICPOdometry : public rtabmap_ros::OdometryROS +{ +public: + ICPOdometry() : + OdometryROS(false, false, true), + scanCloudMaxPoints_(0), + scanDownsamplingStep_(1), + scanVoxelSize_(0.0), + scanNormalK_(0), + scanNormalRadius_(0.0) + { + } + + virtual ~ICPOdometry() + { + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); + pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_); + pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); + pnh.param("scan_normal_k", scanNormalK_, scanNormalK_); + if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k")) + { + ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". " + "The value is still used. Use \"scan_normal_k\" to avoid this warning."); + pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_); + } + pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_); + + NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); + NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_); + NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_); + NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_); + NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_); + + scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this); + cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("1") != 0) + { + ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1")); + + ros::NodeHandle & pnh = getPrivateNodeHandle(); + iter = parameters.find(Parameters::kIcpDownsamplingStep()); + if(iter != parameters.end()) + { + int value = uStr2Int(iter->second); + if(value > 1) + { + if(!pnh.hasParam("scan_downsampling_step")) + { + ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str()); + scanDownsamplingStep_ = value; + iter->second = "1"; + } + else + { + ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str()); + } + } + } + iter = parameters.find(Parameters::kIcpVoxelSize()); + if(iter != parameters.end()) + { + float value = uStr2Float(iter->second); + if(value != 0.0f) + { + if(!pnh.hasParam("scan_voxel_size")) + { + ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str()); + scanVoxelSize_ = value; + iter->second = "0"; + } + else + { + ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str()); + } + } + } + iter = parameters.find(Parameters::kIcpPointToPlaneK()); + if(iter != parameters.end()) + { + int value = uStr2Int(iter->second); + if(value != 0) + { + if(!pnh.hasParam("scan_normal_k")) + { + ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str()); + scanNormalK_ = value; + } + } + } + iter = parameters.find(Parameters::kIcpPointToPlaneRadius()); + if(iter != parameters.end()) + { + float value = uStr2Float(iter->second); + if(value != 0.0f) + { + if(!pnh.hasParam("scan_normal_radius")) + { + ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str()); + scanNormalRadius_ = value; + } + } + } + } + + void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) + { + // make sure the frame of the laser is updated too + Transform localScanTransform = getTransform(this->frameId(), + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); + return; + } + + //transform in frameId_ frame + sensor_msgs::PointCloud2 scanOut; + laser_geometry::LaserProjection projection; + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(scanOut, *pclScan); + pclScan->is_dense = true; + + cv::Mat scan; + int maxLaserScans = (int)scanMsg->ranges.size(); + if(pclScan->size()) + { + if(scanDownsamplingStep_ > 1) + { + pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); + maxLaserScans /= scanDownsamplingStep_; + } + if(scanVoxelSize_ > 0.0f) + { + float pointsBeforeFiltering = (float)pclScan->size(); + pclScan = util3d::voxelize(pclScan, scanVoxelSize_); + float ratio = float(pclScan->size()) / pointsBeforeFiltering; + maxLaserScans = int(float(maxLaserScans) * ratio); + } + if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + { + //compute normals + pcl::PointCloud::Ptr normals; + if(scanVoxelSize_ > 0.0f) + { + normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); + } + else + { + normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_); + } + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + } + + rtabmap::SensorData data( + LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform), + cv::Mat(), + cv::Mat(), + CameraModel(), + 0, + rtabmap_ros::timestampFromROS(scanMsg->header.stamp)); + + this->processData(data, scanMsg->header.stamp); + } + + void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + cv::Mat scan; + bool containNormals = false; + if(scanVoxelSize_ == 0.0f) + { + for(unsigned int i=0; ifields.size(); ++i) + { + if(cloudMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + } + + Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); + return; + } + + int maxLaserScans = scanCloudMaxPoints_; + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + if(pclScan->size() && scanDownsamplingStep_ > 1) + { + pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); + maxLaserScans /= scanDownsamplingStep_; + } + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + if(pclScan->size() && scanDownsamplingStep_ > 1) + { + pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); + maxLaserScans /= scanDownsamplingStep_; + } + if(!pclScan->is_dense) + { + pclScan = util3d::removeNaNFromPointCloud(pclScan); + } + + if(pclScan->size()) + { + if(scanVoxelSize_ > 0.0f) + { + float pointsBeforeFiltering = (float)pclScan->size(); + pclScan = util3d::voxelize(pclScan, scanVoxelSize_); + float ratio = float(pclScan->size()) / pointsBeforeFiltering; + maxLaserScans = int(float(maxLaserScans) * ratio); + } + if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + { + //compute normals + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScanFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScanFromPointCloud(*pclScan); + } + } + } + + rtabmap::SensorData data( + LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform), + cv::Mat(), + cv::Mat(), + CameraModel(), + 0, + rtabmap_ros::timestampFromROS(cloudMsg->header.stamp)); + + this->processData(data, cloudMsg->header.stamp); + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + } + +private: + ros::Subscriber scan_sub_; + ros::Subscriber cloud_sub_; + int scanCloudMaxPoints_; + int scanDownsamplingStep_; + double scanVoxelSize_; + int scanNormalK_; + double scanNormalRadius_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet); + +} diff --git a/rtabmap_demo/src/nodelets/obstacles_detection.cpp b/rtabmap_demo/src/nodelets/obstacles_detection.cpp new file mode 100644 index 0000000..81c3340 --- /dev/null +++ b/rtabmap_demo/src/nodelets/obstacles_detection.cpp @@ -0,0 +1,411 @@ +/* +Copyright (c) 2010-2016, 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 "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/utilite/UStl.h" + +namespace rtabmap_ros +{ + +class ObstaclesDetection : public nodelet::Nodelet +{ +public: + ObstaclesDetection() : + frameId_("base_link"), + waitForTransform_(false), + mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()) + {} + + virtual ~ObstaclesDetection() + {} + +private: + + void parameterMoved( + ros::NodeHandle & nh, + const std::string & rosName, + const std::string & parameterName, + rtabmap::ParametersMap & parameters) + { + if(nh.hasParam(rosName)) + { + rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); + rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName); + if(iter != gridParameters.end()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from " + "rtabmap_ros to rtabmap library. Use " + "parameter \"%s\" instead. The value is still " + "copied to new parameter name.", + rosName.c_str(), + parameterName.c_str()); + std::string type = rtabmap::Parameters::getType(parameterName); + if(type.compare("float") || type.compare("double")) + { + double v = uStr2Double(iter->second); + nh.getParam(rosName, v); + parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); + } + else if(type.compare("int") || type.compare("unsigned int")) + { + int v = uStr2Int(iter->second); + nh.getParam(rosName, v); + parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); + } + else + { + NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); + } + } + else + { + NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); + } + } + } + + virtual void onInit() + { + ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!? + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("frame_id", frameId_, frameId_); + pnh.param("map_frame_id", mapFrameId_, mapFrameId_); + pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); + + if(pnh.hasParam("optimize_for_close_objects")) + { + NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist " + "anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use " + "the old interface."); + } + + rtabmap::ParametersMap parameters; + + // Backward compatibility + for(std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin(); + iter!=rtabmap::Parameters::getRemovedParameters().end(); + ++iter) + { + std::string vStr; + if(pnh.getParam(iter->first, vStr)) + { + if(iter->second.first) + { + // can be migrated + uInsert(parameters, rtabmap::ParametersPair(iter->second.second, vStr)); + NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); + } + else + { + if(iter->second.second.empty()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } + } + } + } + + rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters(); + rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); + for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter) + { + std::string vStr; + bool vBool; + int vInt; + double vDouble; + if(pnh.getParam(iter->first, vStr)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + else if(pnh.getParam(iter->first, vBool)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + iter->second = uBool2Str(vBool); + } + else if(pnh.getParam(iter->first, vDouble)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + iter->second = uNumber2Str(vDouble); + } + else if(pnh.getParam(iter->first, vInt)) + { + NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + iter->second = uNumber2Str(vInt); + } + } + uInsert(parameters, gridParameters); + parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters); + parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters); + parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); + parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); + parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters); + parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters); + parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters); + + UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection())); + mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())); + if(mapFrameProjection_ && mapFrameId_.empty()) + { + NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); + } + + grid_.parseParameters(parameters); + + cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); + + groundPub_ = nh.advertise("ground", 1); + obstaclesPub_ = nh.advertise("obstacles", 1); + projObstaclesPub_ = nh.advertise("proj_obstacles", 1); + } + + + + void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + ros::WallTime time = ros::WallTime::now(); + + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) + { + // no one wants the results + return; + } + + rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + { + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + localTransform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + NODELET_ERROR("%s",ex.what()); + return; + } + + rtabmap::Transform pose = rtabmap::Transform::getIdentity(); + if(!mapFrameId_.empty()) + { + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1))) + { + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp); + pose = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + NODELET_ERROR("%s",ex.what()); + return; + } + } + + pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *inputCloud); + + //Common variables for all strategies + pcl::IndicesPtr ground, obstacles; + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); + + if(inputCloud->size()) + { + inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); + + pcl::IndicesPtr flatObstacles(new std::vector); + pcl::PointCloud::Ptr cloud = grid_.segmentCloud( + inputCloud, + pcl::IndicesPtr(new std::vector), + pose, + cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()), + ground, + obstacles, + &flatObstacles); + + if(cloud->size() && (ground->size() || obstacles->size())) + { + if(groundPub_.getNumSubscribers() && + ground.get() && ground->size()) + { + pcl::copyPointCloud(*cloud, *ground, *groundCloud); + } + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && + obstacles.get() && obstacles->size()) + { + // remove flat obstacles from obstacles + std::set flatObstaclesSet; + if(projObstaclesPub_.getNumSubscribers()) + { + flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); + } + + obstaclesCloud->resize(obstacles->size()); + obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); + + int oi=0; + for(unsigned int i=0; isize(); ++i) + { + obstaclesCloud->points[i] = cloud->at(obstacles->at(i)); + if(flatObstaclesSet.size() == 0 || + flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) + { + obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; + obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; + ++oi; + } + + } + + obstaclesCloudWithoutFlatSurfaces->resize(oi); + } + + if(!localTransform.isIdentity() || !pose.isIdentity()) + { + //transform back in topic frame for 3d clouds and base frame for 2d clouds + + float roll, pitch, yaw; + pose.getEulerAngles(roll, pitch, yaw); + rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0); + + if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity()) + { + obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse()); + } + + t = (t*localTransform).inverse(); + if(groundCloud->size()) + { + groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t); + } + if(obstaclesCloud->size()) + { + obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t); + } + } + } + } + + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*groundCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + obstaclesPub_.publish(rosCloud); + } + + if(projObstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + projObstaclesPub_.publish(rosCloud); + } + + NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); + } + +private: + std::string frameId_; + std::string mapFrameId_; + bool waitForTransform_; + + rtabmap::OccupancyGrid grid_; + bool mapFrameProjection_; + + tf::TransformListener tfListener_; + + ros::Publisher groundPub_; + ros::Publisher obstaclesPub_; + ros::Publisher projObstaclesPub_; + + ros::Subscriber cloudSub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet); +} + + diff --git a/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp b/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp new file mode 100644 index 0000000..40bcd47 --- /dev/null +++ b/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp @@ -0,0 +1,357 @@ +/* +Copyright (c) 2010-2016, 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 "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_mapping.h" +#include "rtabmap/core/util3d_transforms.h" + +namespace rtabmap_ros +{ + +class ObstaclesDetectionOld : public nodelet::Nodelet +{ +public: + ObstaclesDetectionOld() : + frameId_("base_link"), + normalKSearch_(20), + groundNormalAngle_(M_PI_4), + clusterRadius_(0.05), + minClusterSize_(20), + maxObstaclesHeight_(0.0), // if<=0.0 -> disabled + maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true + segmentFlatObstacles_(false), + waitForTransform_(false), + optimizeForCloseObjects_(false), + projVoxelSize_(0.01) + {} + + virtual ~ObstaclesDetectionOld() + {} + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("frame_id", frameId_, frameId_); + pnh.param("normal_k", normalKSearch_, normalKSearch_); + pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); + if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius")) + { + NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed " + "to \"cluster_radius\"! Your value is still copied to " + "corresponding parameter. Instead of normal radius, nearest neighbors count " + "\"normal_k\" is used instead (default 20)."); + pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_); + } + else + { + pnh.param("cluster_radius", clusterRadius_, clusterRadius_); + } + pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); + pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_); + pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_); + pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); + pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); + pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_); + + cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this); + + groundPub_ = nh.advertise("ground", 1); + obstaclesPub_ = nh.advertise("obstacles", 1); + projObstaclesPub_ = nh.advertise("proj_obstacles", 1); + } + + + + void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) + { + ros::WallTime time = ros::WallTime::now(); + + if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) + { + // no one wants the results + return; + } + + rtabmap::Transform localTransform; + try + { + if(waitForTransform_) + { + if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) + { + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + return; + } + } + tf::StampedTransform tmp; + tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); + localTransform = rtabmap_ros::transformFromTF(tmp); + } + catch(tf::TransformException & ex) + { + NODELET_ERROR("%s",ex.what()); + return; + } + + pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *originalCloud); + + //Common variables for all strategies + pcl::IndicesPtr ground, obstacles; + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); + + if(originalCloud->size()) + { + originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); + if(maxObstaclesHeight_ > 0) + { + // std::numeric_limits::lowest() exists only for c++11 + originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); + } + + if(originalCloud->size()) + { + if(!optimizeForCloseObjects_) + { + // This is the default strategy + pcl::IndicesPtr flatObstacles(new std::vector); + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud, + ground, + obstacles, + normalKSearch_, + groundNormalAngle_, + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_, + &flatObstacles); + + if(groundPub_.getNumSubscribers() && + ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); + } + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && + obstacles.get() && obstacles->size()) + { + // remove flat obstacles from obstacles + std::set flatObstaclesSet; + if(projObstaclesPub_.getNumSubscribers()) + { + flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); + } + + obstaclesCloud->resize(obstacles->size()); + obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); + + int oi=0; + for(unsigned int i=0; isize(); ++i) + { + obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i)); + if(flatObstaclesSet.size() == 0 || + flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) + { + obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; + obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; + ++oi; + } + + } + + obstaclesCloudWithoutFlatSurfaces->resize(oi); + if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0) + { + obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_); + } + } + } + else + { + // in this case optimizeForCloseObject_ is true: + // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m + // one for potential floor points further away than 1m. + // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, + // which allows to detect smaller objects, without increasing the number of false positive. + // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the + // grond normal angle (* 2.). + + pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); + pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); + + // Part 1: segment floor and obstacles near the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_near, + ground, + obstacles, + normalKSearch_, + groundNormalAngle_, + clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); + ground->clear(); + } + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + { + pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); + obstacles->clear(); + } + + // Part 2: segment floor and obstacles far from the robot + rtabmap::util3d::segmentObstaclesFromGround( + originalCloud_far, + ground, + obstacles, + normalKSearch_, + 2.*groundNormalAngle_, + 3.*clusterRadius_, + minClusterSize_, + segmentFlatObstacles_, + maxGroundHeight_); + + if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) + { + pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); + *groundCloud += *groundCloud2; + } + + + if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) + { + pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); + pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); + *obstaclesCloud += *obstacles2; + } + } + + if(!localTransform.isIdentity()) + { + //transform back in topic frame + rtabmap::Transform localTransformInv = localTransform.inverse(); + if(groundCloud->size()) + { + groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv); + } + if(obstaclesCloud->size()) + { + obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv); + } + } + } + } + + if(groundPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*groundCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + groundPub_.publish(rosCloud); + } + + if(obstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloud, rosCloud); + rosCloud.header = cloudMsg->header; + + //publish the message + obstaclesPub_.publish(rosCloud); + } + + if(projObstaclesPub_.getNumSubscribers()) + { + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); + rosCloud.header.stamp = cloudMsg->header.stamp; + rosCloud.header.frame_id = frameId_; + + //publish the message + projObstaclesPub_.publish(rosCloud); + } + + NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); + } + +private: + std::string frameId_; + int normalKSearch_; + double groundNormalAngle_; + double clusterRadius_; + int minClusterSize_; + double maxObstaclesHeight_; + double maxGroundHeight_; + bool segmentFlatObstacles_; + bool waitForTransform_; + bool optimizeForCloseObjects_; + double projVoxelSize_; + + tf::TransformListener tfListener_; + + ros::Publisher groundPub_; + ros::Publisher obstaclesPub_; + ros::Publisher projObstaclesPub_; + + ros::Subscriber cloudSub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetectionOld, nodelet::Nodelet); +} + + diff --git a/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp new file mode 100644 index 0000000..bcdc776 --- /dev/null +++ b/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp @@ -0,0 +1,112 @@ + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +namespace rtabmap_ros +{ + +class PointCloudAggregator : public nodelet::Nodelet +{ +public: + PointCloudAggregator() : sync_(NULL) + {} + + virtual ~PointCloudAggregator() + { + if (sync_!=NULL) delete sync_; + } + +private: + void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2, + const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3) + { + if(cloudPub_.getNumSubscribers()) + { + pcl::PointCloud cloud1, cloud2, cloud3; + + pcl::fromROSMsg(*cloudMsg_1, cloud1); + std::string frameId = frameId_; + if(!frameId.empty() && frameId.compare(cloudMsg_1->header.frame_id) != 0) + { + pcl_ros::transformPointCloud(frameId, cloud1, cloud1, tfListener_); + } + else + { + frameId = cloudMsg_1->header.frame_id; + } + pcl::fromROSMsg(*cloudMsg_2, cloud2); + if(frameId.compare(cloudMsg_2->header.frame_id) != 0) + { + pcl_ros::transformPointCloud(frameId, cloud2, cloud2, tfListener_); + } + pcl::fromROSMsg(*cloudMsg_3, cloud3); + if(frameId.compare(cloudMsg_3->header.frame_id) != 0) + { + pcl_ros::transformPointCloud(frameId, cloud3, cloud3, tfListener_); + } + pcl::PointCloud totalCloud; + totalCloud = cloud1 + cloud2; + totalCloud += cloud3; + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(totalCloud, rosCloud); + rosCloud.header.stamp = cloudMsg_1->header.stamp; + rosCloud.header.frame_id = frameId; + cloudPub_.publish(rosCloud); + } + } + + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 5; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("frame_id", frameId_, frameId_); + + cloudSub_1_.subscribe(nh, "cloud1", 1); + cloudSub_2_.subscribe(nh, "cloud2", 1); + cloudSub_3_.subscribe(nh, "cloud3", 1); + + sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + sync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3)); + + cloudPub_ = nh.advertise("combined_cloud", 1); + } + + message_filters::Synchronizer* sync_; + message_filters::Subscriber cloudSub_1_; + message_filters::Subscriber cloudSub_2_; + message_filters::Subscriber cloudSub_3_; + + ros::Publisher cloudPub_; + + std::string frameId_; + tf::TransformListener tfListener_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet); +} + diff --git a/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp b/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp new file mode 100644 index 0000000..0b322d3 --- /dev/null +++ b/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp @@ -0,0 +1,377 @@ +/* +Copyright (c) 2010-2016, 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 +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_surface.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UStl.h" + +namespace rtabmap_ros +{ + +class PointCloudXYZ : public nodelet::Nodelet +{ +public: + PointCloudXYZ() : + maxDepth_(0.0), + minDepth_(0.0), + voxelSize_(0.0), + decimation_(1), + noiseFilterRadius_(0.0), + noiseFilterMinNeighbors_(5), + normalK_(0), + normalRadius_(0.0), + filterNaNs_(false), + approxSyncDepth_(0), + approxSyncDisparity_(0), + exactSyncDepth_(0), + exactSyncDisparity_(0) + {} + + virtual ~PointCloudXYZ() + { + if(approxSyncDepth_) + delete approxSyncDepth_; + if(approxSyncDisparity_) + delete approxSyncDisparity_; + if(exactSyncDepth_) + delete exactSyncDepth_; + if(exactSyncDisparity_) + delete exactSyncDisparity_; + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + bool approxSync = true; + std::string roiStr; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize, queueSize); + pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); + pnh.param("voxel_size", voxelSize_, voxelSize_); + pnh.param("decimation", decimation_, decimation_); + pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); + pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); + pnh.param("normal_k", normalK_, normalK_); + pnh.param("normal_radius", normalRadius_, normalRadius_); + pnh.param("filter_nans", filterNaNs_, filterNaNs_); + pnh.param("roi_ratios", roiStr, roiStr); + + // Deprecated + if(pnh.hasParam("cut_left")) + { + ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored."); + } + if(pnh.hasParam("cut_right")) + { + ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored."); + } + if(pnh.hasParam("special_filter_close_object")) + { + ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing " + "should be done before or after this nodelet. See old implementation here: " + "https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201."); + } + + //parse roi (region of interest) + roiRatios_.resize(4, 0); + if(!roiStr.empty()) + { + std::list strValues = uSplit(roiStr, ' '); + if(strValues.size() != 4) + { + ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str()); + } + else + { + std::vector tmpValues(4); + unsigned int i=0; + for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) + { + tmpValues[i] = uStr2Float(*jter); + ++i; + } + + if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && + tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && + tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && + tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) + { + roiRatios_ = tmpValues; + } + else + { + ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str()); + } + } + } + + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + if(approxSync) + { + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); + + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); + } + else + { + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); + + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); + } + + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); + + disparitySub_.subscribe(nh, "disparity/image", 1); + disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); + + cloudPub_ = nh.advertise("cloud", 1); + } + + void callback( + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) + { + NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); + return; + } + + if(cloudPub_.getNumSubscribers()) + { + ros::WallTime time = ros::WallTime::now(); + + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); + + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(*cameraInfo); + + pcl::PointCloud::Ptr pclCloud; + rtabmap::CameraModel m( + model.fx(), + model.fy(), + model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), + model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + + pcl::IndicesPtr indices(new std::vector); + pclCloud = rtabmap::util3d::cloudFromDepth( + cv::Mat(imageDepthPtr->image, roi), + m, + decimation_, + maxDepth_, + minDepth_, + indices.get()); + processAndPublish(pclCloud, indices, depth->header); + + NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); + } + } + + void callbackDisparity( + const stereo_msgs::DisparityImageConstPtr& disparityMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && + disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) + { + NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); + return; + } + + cv::Mat disparity; + if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0) + { + disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); + } + else + { + disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast(disparityMsg->image.data.data())); + } + + if(cloudPub_.getNumSubscribers()) + { + ros::WallTime time = ros::WallTime::now(); + + cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_); + + pcl::PointCloud::Ptr pclCloud; + rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T); + pcl::IndicesPtr indices(new std::vector); + pclCloud = rtabmap::util3d::cloudFromDisparity( + cv::Mat(disparity, roi), + stereoModel, + decimation_, + maxDepth_, + minDepth_, + indices.get()); + + processAndPublish(pclCloud, indices, disparityMsg->header); + + NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec()); + } + } + + void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) + { + if(indices->size() && voxelSize_ > 0.0) + { + pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); + } + + // Do radius filtering after voxel filtering ( a lot faster) + if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) + { + if(pclCloud->is_dense) + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + else + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*pclCloud, *indices, *tmp); + pclCloud = tmp; + } + + sensor_msgs::PointCloud2 rosCloud; + if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) + { + //compute normals + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); + pcl::PointCloud::Ptr pclCloudNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal); + if(filterNaNs_) + { + pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); + } + pcl::toROSMsg(*pclCloudNormal, rosCloud); + } + else + { + if(filterNaNs_ && !pclCloud->is_dense) + { + pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); + } + pcl::toROSMsg(*pclCloud, rosCloud); + } + rosCloud.header.stamp = header.stamp; + rosCloud.header.frame_id = header.frame_id; + + //publish the message + cloudPub_.publish(rosCloud); + } + +private: + + double maxDepth_; + double minDepth_; + double voxelSize_; + int decimation_; + double noiseFilterRadius_; + int noiseFilterMinNeighbors_; + int normalK_; + double normalRadius_; + bool filterNaNs_; + std::vector roiRatios_; + + ros::Publisher cloudPub_; + + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + + message_filters::Subscriber disparitySub_; + message_filters::Subscriber disparityCameraInfoSub_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; + message_filters::Synchronizer * approxSyncDepth_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDisparityPolicy; + message_filters::Synchronizer * approxSyncDisparity_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; + message_filters::Synchronizer * exactSyncDepth_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDisparityPolicy; + message_filters::Synchronizer * exactSyncDisparity_; + +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet); +} + diff --git a/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp new file mode 100644 index 0000000..091d1a2 --- /dev/null +++ b/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp @@ -0,0 +1,539 @@ +/* +Copyright (c) 2010-2016, 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 +#include + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_surface.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UStl.h" + +namespace rtabmap_ros +{ + +class PointCloudXYZRGB : public nodelet::Nodelet +{ +public: + PointCloudXYZRGB() : + maxDepth_(0.0), + minDepth_(0.0), + voxelSize_(0.0), + decimation_(1), + noiseFilterRadius_(0.0), + noiseFilterMinNeighbors_(5), + normalK_(0), + normalRadius_(0.0), + filterNaNs_(false), + approxSyncDepth_(0), + approxSyncDisparity_(0), + approxSyncStereo_(0), + exactSyncDepth_(0), + exactSyncDisparity_(0), + exactSyncStereo_(0) + {} + + virtual ~PointCloudXYZRGB() + { + if(approxSyncDepth_) + delete approxSyncDepth_; + if(approxSyncDisparity_) + delete approxSyncDisparity_; + if(approxSyncStereo_) + delete approxSyncStereo_; + if(exactSyncDepth_) + delete exactSyncDepth_; + if(exactSyncDisparity_) + delete exactSyncDisparity_; + if(exactSyncStereo_) + delete exactSyncStereo_; + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + bool approxSync = true; + std::string roiStr; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize, queueSize); + pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); + pnh.param("voxel_size", voxelSize_, voxelSize_); + pnh.param("decimation", decimation_, decimation_); + pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); + pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); + pnh.param("normal_k", normalK_, normalK_); + pnh.param("normal_radius", normalRadius_, normalRadius_); + pnh.param("filter_nans", filterNaNs_, filterNaNs_); + pnh.param("roi_ratios", roiStr, roiStr); + + //parse roi (region of interest) + roiRatios_.resize(4, 0); + if(!roiStr.empty()) + { + std::list strValues = uSplit(roiStr, ' '); + if(strValues.size() != 4) + { + ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str()); + } + else + { + std::vector tmpValues(4); + unsigned int i=0; + for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) + { + tmpValues[i] = uStr2Float(*jter); + ++i; + } + + if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && + tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && + tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && + tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) + { + roiRatios_ = tmpValues; + } + else + { + ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str()); + } + } + } + + // StereoBM parameters + stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM"); + for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter) + { + std::string vStr; + bool vBool; + int vInt; + double vDouble; + if(pnh.getParam(iter->first, vStr)) + { + NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + else if(pnh.getParam(iter->first, vBool)) + { + NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); + iter->second = uBool2Str(vBool); + } + else if(pnh.getParam(iter->first, vDouble)) + { + NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); + iter->second = uNumber2Str(vDouble); + } + else if(pnh.getParam(iter->first, vInt)) + { + NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); + iter->second = uNumber2Str(vInt); + } + } + + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + cloudPub_ = nh.advertise("cloud", 1); + + if(approxSync) + { + + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3)); + + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3)); + + approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4)); + } + else + { + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3)); + + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3)); + + exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4)); + } + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + ros::NodeHandle left_pnh(pnh, "left"); + ros::NodeHandle right_pnh(pnh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); + image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); + + imageDisparitySub_.subscribe(nh, "disparity", 1); + + imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); + imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); + cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + } + + void depthCallback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& imageDepth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || + !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || + imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || + imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); + return; + } + + if(cloudPub_.getNumSubscribers()) + { + ros::WallTime time = ros::WallTime::now(); + + cv_bridge::CvImageConstPtr imagePtr; + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } + + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); + + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(*cameraInfo); + + ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols); + ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows); + + pcl::PointCloud::Ptr pclCloud; + cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); + + rtabmap::CameraModel m( + model.fx(), + model.fy(), + model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), + model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + pcl::IndicesPtr indices(new std::vector); + pclCloud = rtabmap::util3d::cloudFromDepthRGB( + cv::Mat(imagePtr->image, roi), + cv::Mat(imageDepthPtr->image, roi), + m, + decimation_, + maxDepth_, + minDepth_, + indices.get()); + + + processAndPublish(pclCloud, indices, imagePtr->header); + + NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec()); + } + } + + void disparityCallback( + const sensor_msgs::ImageConstPtr& image, + const stereo_msgs::DisparityImageConstPtr& imageDisparity, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + cv_bridge::CvImageConstPtr imagePtr; + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } + + if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && + imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) + { + NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); + return; + } + + cv::Mat disparity; + if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0) + { + disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast(imageDisparity->image.data.data())); + } + else + { + disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast(imageDisparity->image.data.data())); + } + + if(cloudPub_.getNumSubscribers()) + { + ros::WallTime time = ros::WallTime::now(); + + cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_); + + pcl::PointCloud::Ptr pclCloud; + rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T); + pcl::IndicesPtr indices(new std::vector); + pclCloud = rtabmap::util3d::cloudFromDisparityRGB( + cv::Mat(imagePtr->image, roi), + cv::Mat(disparity, roi), + stereoModel, + decimation_, + maxDepth_, + minDepth_, + indices.get()); + + processAndPublish(pclCloud, indices, imageDisparity->header); + + NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec()); + } + } + + void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft, + const sensor_msgs::ImageConstPtr& imageRight, + const sensor_msgs::CameraInfoConstPtr& camInfoLeft, + const sensor_msgs::CameraInfoConstPtr& camInfoRight) + { + if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || + imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); + return; + } + + if(cloudPub_.getNumSubscribers()) + { + ros::WallTime time = ros::WallTime::now(); + + cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; + if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); + } + else + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); + } + ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); + + if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f) + { + ROS_WARN("\"roi_ratios\" set but ignored for stereo images."); + } + + pcl::PointCloud::Ptr pclCloud; + pcl::IndicesPtr indices(new std::vector); + pclCloud = rtabmap::util3d::cloudFromStereoImages( + ptrLeftImage->image, + ptrRightImage->image, + rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), + decimation_, + maxDepth_, + minDepth_, + indices.get(), + stereoBMParameters_); + + processAndPublish(pclCloud, indices, imageLeft->header); + + NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec()); + } + } + + void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) + { + if(indices->size() && voxelSize_ > 0.0) + { + pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); + } + + // Do radius filtering after voxel filtering ( a lot faster) + if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) + { + if(pclCloud->is_dense) + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + else + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*pclCloud, *indices, *tmp); + pclCloud = tmp; + } + + sensor_msgs::PointCloud2 rosCloud; + if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) + { + //compute normals + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); + pcl::PointCloud::Ptr pclCloudNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal); + if(filterNaNs_) + { + pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); + } + pcl::toROSMsg(*pclCloudNormal, rosCloud); + } + else + { + if(filterNaNs_ && !pclCloud->is_dense) + { + pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); + } + pcl::toROSMsg(*pclCloud, rosCloud); + } + rosCloud.header.stamp = header.stamp; + rosCloud.header.frame_id = header.frame_id; + + //publish the message + cloudPub_.publish(rosCloud); + } + +private: + + double maxDepth_; + double minDepth_; + double voxelSize_; + int decimation_; + double noiseFilterRadius_; + int noiseFilterMinNeighbors_; + int normalK_; + double normalRadius_; + bool filterNaNs_; + std::vector roiRatios_; + rtabmap::ParametersMap stereoBMParameters_; + + ros::Publisher cloudPub_; + + image_transport::SubscriberFilter imageSub_; + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + + message_filters::Subscriber imageDisparitySub_; + + image_transport::SubscriberFilter imageLeft_; + image_transport::SubscriberFilter imageRight_; + message_filters::Subscriber cameraInfoLeft_; + message_filters::Subscriber cameraInfoRight_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; + message_filters::Synchronizer * approxSyncDepth_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDisparityPolicy; + message_filters::Synchronizer * approxSyncDisparity_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncStereoPolicy; + message_filters::Synchronizer * approxSyncStereo_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; + message_filters::Synchronizer * exactSyncDepth_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDisparityPolicy; + message_filters::Synchronizer * exactSyncDisparity_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncStereoPolicy; + message_filters::Synchronizer * exactSyncStereo_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet); +} + diff --git a/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp new file mode 100644 index 0000000..d4db77a --- /dev/null +++ b/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp @@ -0,0 +1,247 @@ +/* +Copyright (c) 2010-2016, 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 +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +namespace rtabmap_ros +{ + +class PointCloudToDepthImage : public nodelet::Nodelet +{ +public: + PointCloudToDepthImage() : + listener_(0), + waitForTransform_(0.1), + fillHolesSize_ (0), + fillHolesError_(0.1), + fillIterations_(1), + decimation_(1), + approxSync_(0), + exactSync_(0) + {} + + virtual ~PointCloudToDepthImage() + { + delete listener_; + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + } + +private: + virtual void onInit() + { + listener_ = new tf::TransformListener(); + + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + bool approx = true; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); + pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); + pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_); + pnh.param("fill_holes_error", fillHolesError_, fillHolesError_); + pnh.param("fill_iterations", fillIterations_, fillIterations_); + pnh.param("decimation", decimation_, decimation_); + pnh.param("approx", approx, approx); + + if(fixedFrameId_.empty() && approx) + { + ROS_FATAL("fixed_frame_id should be set when using approximate " + "time synchronization (approx=true)! If the robot " + "is moving, it could be \"odom\". If not moving, it " + "could be \"base_link\"."); + } + + ROS_INFO("Params:"); + ROS_INFO(" approx=%s", approx?"true":"false"); + ROS_INFO(" queue_size=%d", queueSize); + ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str()); + ROS_INFO(" wait_for_transform=%fs", waitForTransform_); + ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); + ROS_INFO(" fill_holes_error=%f", fillHolesError_); + ROS_INFO(" fill_iterations=%d", fillIterations_); + ROS_INFO(" decimation=%d", decimation_); + + image_transport::ImageTransport it(nh); + depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm + depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters + + if(approx) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2)); + } + else + { + fixedFrameId_.clear(); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2)); + } + + pointCloudSub_.subscribe(nh, "cloud", 1); + cameraInfoSub_.subscribe(nh, "camera_info", 1); + } + + void callback( + const sensor_msgs::PointCloud2ConstPtr & pointCloud2Msg, + const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg) + { + if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0) + { + rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity(); + if(!fixedFrameId_.empty()) + { + // approx sync + cloudDisplacement = rtabmap_ros::getTransform( + pointCloud2Msg->header.frame_id, + fixedFrameId_, + pointCloud2Msg->header.stamp, + cameraInfoMsg->header.stamp, + *listener_, + waitForTransform_); + } + + if(cloudDisplacement.isNull()) + { + return; + } + + rtabmap::Transform cloudToCamera = rtabmap_ros::getTransform( + pointCloud2Msg->header.frame_id, + cameraInfoMsg->header.frame_id, + cameraInfoMsg->header.stamp, + *listener_, + waitForTransform_); + + if(cloudToCamera.isNull()) + { + return; + } + + rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera; + + rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform); + + if(decimation_ > 1) + { + if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0) + { + model = model.scaled(1.0f/float(decimation_)); + } + else + { + ROS_ERROR("decimation (%d) not valid for image size %dx%d", + decimation_, + model.imageWidth(), + model.imageHeight()); + } + } + + pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*pointCloud2Msg, *cloud); + + cv_bridge::CvImage depthImage; + depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform()); + + if(fillHolesSize_ > 0 && fillIterations_ > 0) + { + for(int i=0; iheader; + + if(depthImage32Pub_.getNumSubscribers()) + { + depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + depthImage32Pub_.publish(depthImage.toImageMsg()); + } + + if(depthImage16Pub_.getNumSubscribers()) + { + depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image); + depthImage16Pub_.publish(depthImage.toImageMsg()); + } + } + } + +private: + image_transport::Publisher depthImage16Pub_; + image_transport::Publisher depthImage32Pub_; + message_filters::Subscriber pointCloudSub_; + message_filters::Subscriber cameraInfoSub_; + std::string fixedFrameId_; + tf::TransformListener * listener_; + double waitForTransform_; + int fillHolesSize_; + double fillHolesError_; + int fillIterations_; + int decimation_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudToDepthImage, nodelet::Nodelet); +} diff --git a/rtabmap_demo/src/nodelets/rgbd_odometry.cpp b/rtabmap_demo/src/nodelets/rgbd_odometry.cpp new file mode 100644 index 0000000..f71ce5b --- /dev/null +++ b/rtabmap_demo/src/nodelets/rgbd_odometry.cpp @@ -0,0 +1,654 @@ +/* +Copyright (c) 2010-2016, 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 +#include +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class RGBDOdometry : public rtabmap_ros::OdometryROS +{ +public: + RGBDOdometry() : + OdometryROS(false, true, false), + approxSync_(0), + exactSync_(0), + approxSync2_(0), + exactSync2_(0), + approxSync3_(0), + exactSync3_(0), + approxSync4_(0), + exactSync4_(0), + queueSize_(5) + { + } + + virtual ~RGBDOdometry() + { + rgbdSub_.shutdown(); + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + if(approxSync2_) + { + delete approxSync2_; + } + if(exactSync2_) + { + delete exactSync2_; + } + if(approxSync3_) + { + delete approxSync3_; + } + if(exactSync3_) + { + delete exactSync3_; + } + if(approxSync4_) + { + delete approxSync4_; + } + if(exactSync4_) + { + delete exactSync4_; + } + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int rgbdCameras = 1; + bool approxSync = true; + bool subscribeRGBD = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); + if(pnh.hasParam("depth_cameras")) + { + ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true."); + } + pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); + if(rgbdCameras <= 0) + { + rgbdCameras = 1; + } + if(rgbdCameras > 4) + { + NODELET_FATAL("Only 4 cameras maximum supported yet."); + } + + NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); + NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); + NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras); + + std::string subscribedTopicsMsg; + if(subscribeRGBD) + { + if(rgbdCameras >= 2) + { + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + if(rgbdCameras >= 3) + { + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + } + if(rgbdCameras >= 4) + { + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + } + + if(rgbdCameras == 2) + { + if(approxSync) + { + approxSync2_ = new message_filters::Synchronizer( + MyApproxSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + else + { + exactSync2_ = new message_filters::Synchronizer( + MyExactSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str()); + } + else if(rgbdCameras == 3) + { + if(approxSync) + { + approxSync3_ = new message_filters::Synchronizer( + MyApproxSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + else + { + exactSync3_ = new message_filters::Synchronizer( + MyExactSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str()); + } + else if(rgbdCameras == 4) + { + if(approxSync) + { + approxSync4_ = new message_filters::Synchronizer( + MyApproxSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + else + { + exactSync4_ = new message_filters::Synchronizer( + MyExactSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str()); + } + } + else + { + rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this); + + subscribedTopicsMsg = + uFormat("\n%s subscribed to:\n %s", + getName().c_str(), + rgbdSub_.getTopic().c_str()); + } + } + else + { + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", 1); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str()); + } + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); + + int estimationType = Parameters::defaultVisEstimationType(); + Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + int rgbdCameras = 1; + bool subscribeRGBD = false; + pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); + pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); + if(subscribeRGBD && rgbdCameras> 1 && estimationType>0) + { + NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported " + "for multi-cameras) as \"subscribe_rgbd\" is " + "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.", + Parameters::kVisEstimationType().c_str(), + estimationType, + Parameters::kVisEstimationType().c_str()); + uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0")); + } + } + + void commonCallback( + const std::vector & rgbImages, + const std::vector & depthImages, + const std::vector& cameraInfos) + { + ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); + ros::Time higherStamp; + int imageWidth = rgbImages[0]->image.cols; + int imageHeight = rgbImages[0]->image.rows; + int depthWidth = depthImages[0]->image.cols; + int depthHeight = depthImages[0]->image.rows; + + UASSERT_MSG( + imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 && + imageWidth/depthWidth == imageHeight/depthHeight, + uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); + + int cameraCount = rgbImages.size(); + cv::Mat rgb; + cv::Mat depth; + pcl::PointCloud scanCloud; + std::vector cameraModels; + for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || + !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || + depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and " + "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s", + rgbImages[i]->encoding.c_str(), + depthImages[i]->encoding.c_str()); + return; + } + UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight, + uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", + imageWidth, + rgbImages[i]->image.cols, + imageHeight, + rgbImages[i]->image.rows).c_str()); + UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight, + uFormat("depthWidth=%d vs %d depthHeight=%d vs %d", + depthWidth, + depthImages[i]->image.cols, + depthHeight, + depthImages[i]->image.rows).c_str()); + + ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp; + + if(i == 0) + { + higherStamp = stamp; + } + else if(stamp > higherStamp) + { + higherStamp = stamp; + } + + Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + cv_bridge::CvImageConstPtr ptrImage = rgbImages[i]; + if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && + rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) + { + ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8"); + } + + cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; + cv::Mat subDepth = ptrDepth->image; + + // initialize + if(rgb.empty()) + { + rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); + } + if(depth.empty()) + { + depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type()); + } + + if(ptrImage->image.type() == rgb.type()) + { + ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); + } + else + { + NODELET_ERROR("Some RGB images are not the same type!"); + return; + } + + if(subDepth.type() == depth.type()) + { + subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); + } + else + { + NODELET_ERROR("Some Depth images are not the same type!"); + return; + } + + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform)); + } + + rtabmap::SensorData data( + rgb, + depth, + cameraModels, + 0, + rtabmap_ros::timestampFromROS(higherStamp)); + + this->processData(data, higherStamp); + } + + void callback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(1); + std::vector depthMsgs(1); + std::vector infoMsgs; + imageMsgs[0] = cv_bridge::toCvShare(image); + depthMsgs[0] = cv_bridge::toCvShare(depth); + infoMsgs.push_back(*cameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + + void callbackRGBD( + const rtabmap_ros::RGBDImageConstPtr& image) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(1); + std::vector depthMsgs(1); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + infoMsgs.push_back(image->rgbCameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + + void callbackRGBD2( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(2); + std::vector depthMsgs(2); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + infoMsgs.push_back(image->rgbCameraInfo); + infoMsgs.push_back(image2->rgbCameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + + void callbackRGBD3( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2, + const rtabmap_ros::RGBDImageConstPtr& image3) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(3); + std::vector depthMsgs(3); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + infoMsgs.push_back(image->rgbCameraInfo); + infoMsgs.push_back(image2->rgbCameraInfo); + infoMsgs.push_back(image3->rgbCameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + + void callbackRGBD4( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2, + const rtabmap_ros::RGBDImageConstPtr& image3, + const rtabmap_ros::RGBDImageConstPtr& image4) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(4); + std::vector depthMsgs(4); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]); + infoMsgs.push_back(image->rgbCameraInfo); + infoMsgs.push_back(image2->rgbCameraInfo); + infoMsgs.push_back(image3->rgbCameraInfo); + infoMsgs.push_back(image4->rgbCameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + if(approxSync_) + { + delete approxSync_; + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + if(exactSync_) + { + delete exactSync_; + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); + } + if(approxSync2_) + { + delete approxSync2_; + approxSync2_ = new message_filters::Synchronizer( + MyApproxSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + if(exactSync2_) + { + delete exactSync2_; + exactSync2_ = new message_filters::Synchronizer( + MyExactSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + if(approxSync3_) + { + delete approxSync3_; + approxSync3_ = new message_filters::Synchronizer( + MyApproxSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + if(exactSync3_) + { + delete exactSync3_; + exactSync3_ = new message_filters::Synchronizer( + MyExactSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + if(approxSync4_) + { + delete approxSync4_; + approxSync4_ = new message_filters::Synchronizer( + MyApproxSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + if(exactSync4_) + { + delete exactSync4_; + exactSync4_ = new message_filters::Synchronizer( + MyExactSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + } + +private: + image_transport::SubscriberFilter image_mono_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + + ros::Subscriber rgbdSub_; + message_filters::Subscriber rgbd_image1_sub_; + message_filters::Subscriber rgbd_image2_sub_; + message_filters::Subscriber rgbd_image3_sub_; + message_filters::Subscriber rgbd_image4_sub_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync2Policy; + message_filters::Synchronizer * approxSync2_; + typedef message_filters::sync_policies::ExactTime MyExactSync2Policy; + message_filters::Synchronizer * exactSync2_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync3Policy; + message_filters::Synchronizer * approxSync3_; + typedef message_filters::sync_policies::ExactTime MyExactSync3Policy; + message_filters::Synchronizer * exactSync3_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync4Policy; + message_filters::Synchronizer * approxSync4_; + typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; + message_filters::Synchronizer * exactSync4_; + int queueSize_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet); + +} diff --git a/rtabmap_demo/src/nodelets/rgbd_sync.cpp b/rtabmap_demo/src/nodelets/rgbd_sync.cpp new file mode 100644 index 0000000..e8ddd5f --- /dev/null +++ b/rtabmap_demo/src/nodelets/rgbd_sync.cpp @@ -0,0 +1,243 @@ +/* +Copyright (c) 2010-2016, 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 +#include +#include + +#include +#include + +#include + +#include "rtabmap_ros/RGBDImage.h" + +#include "rtabmap/core/Compression.h" +#include "rtabmap/utilite/UConversion.h" + +namespace rtabmap_ros +{ + +class RGBDSync : public nodelet::Nodelet +{ +public: + RGBDSync() : + depthScale_(1.0), + warningThread_(0), + callbackCalled_(false), + approxSyncDepth_(0), + exactSyncDepth_(0) + {} + + virtual ~RGBDSync() + { + if(approxSyncDepth_) + delete approxSyncDepth_; + if(exactSyncDepth_) + delete exactSyncDepth_; + + if(warningThread_) + { + callbackCalled_=true; + warningThread_->join(); + delete warningThread_; + } + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + bool approxSync = true; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize, queueSize); + pnh.param("depth_scale", depthScale_, depthScale_); + + NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); + NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_); + + rgbdImagePub_ = nh.advertise("rgbd_image", 1); + rgbdImageCompressedPub_ = nh.advertise("rgbd_image/compressed", 1); + + if(approxSync) + { + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_); + approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4)); + } + else + { + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_); + exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4)); + } + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + cameraDepthInfoSub_.subscribe(depth_nh, "camera_info", 1); + + std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + imageSub_.getTopic().c_str(), + imageDepthSub_.getTopic().c_str(), + cameraInfoSub_.getTopic().c_str(), + cameraDepthInfoSub_.getTopic().c_str()); + + warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync)); + NODELET_INFO("%s", subscribedTopicsMsg.c_str()); + } + + void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync) + { + ros::Duration r(5.0); + while(!callbackCalled_) + { + r.sleep(); + if(!callbackCalled_) + { + ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set. %s%s", + getName().c_str(), + approxSync?"":"Parameter \"approx_sync\" is false, which means that input " + "topics should have all the exact timestamp for the callback to be called.", + subscribedTopicsMsg.c_str()); + } + } + } + + void callback( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::CameraInfoConstPtr& cameraDepthInfo) + { + callbackCalled_ = true; + if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers()) + { + rtabmap_ros::RGBDImage msg; + msg.header.frame_id = cameraInfo->header.frame_id; + msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; + msg.rgbCameraInfo = *cameraInfo; + msg.depthCameraInfo = *cameraDepthInfo; + + if(rgbdImageCompressedPub_.getNumSubscribers()) + { + rtabmap_ros::RGBDImage msgCompressed = msg; + + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); + imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG); + + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + if(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1) + { + msgCompressed.depthCompressed.header = imageDepthPtr->header; + if(depthScale_ != 1.0) + { + msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png"); + } + else + { + msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png"); + } + msgCompressed.depthCompressed.format = "png"; + } + else + { + // Assume right stereo image + imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG); + } + + rgbdImageCompressedPub_.publish(msgCompressed); + } + + if(rgbdImagePub_.getNumSubscribers()) + { + msg.rgb = *image; + if(depthScale_ != 1.0) + { + cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth); + imageDepthPtr->image*=depthScale_; + msg.depth = *imageDepthPtr->toImageMsg(); + } + else + { + msg.depth = *depth; + } + rgbdImagePub_.publish(msg); + } + } + } + +private: + double depthScale_; + boost::thread * warningThread_; + bool callbackCalled_; + + ros::Publisher rgbdImagePub_; + ros::Publisher rgbdImageCompressedPub_; + + image_transport::SubscriberFilter imageSub_; + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + message_filters::Subscriber cameraDepthInfoSub_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; + message_filters::Synchronizer * approxSyncDepth_; + + typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; + message_filters::Synchronizer * exactSyncDepth_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet); +} + diff --git a/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp new file mode 100644 index 0000000..b2ef1e5 --- /dev/null +++ b/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp @@ -0,0 +1,473 @@ +/* +Copyright (c) 2010-2016, 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 + +#include +#include +#include +#include +#include +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class RGBDICPOdometry : public rtabmap_ros::OdometryROS +{ +public: + RGBDICPOdometry() : + OdometryROS(false, true, true), + approxScanSync_(0), + exactScanSync_(0), + approxCloudSync_(0), + exactCloudSync_(0), + queueSize_(5), + scanCloudMaxPoints_(0), + scanVoxelSize_(0.0), + scanNormalK_(0), + scanNormalRadius_(0.0) + { + } + + virtual ~RGBDICPOdometry() + { + if(approxScanSync_) + { + delete approxScanSync_; + } + if(exactScanSync_) + { + delete exactScanSync_; + } + if(approxCloudSync_) + { + delete approxCloudSync_; + } + if(exactCloudSync_) + { + delete exactCloudSync_; + } + } + +private: + + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + bool approxSync = true; + bool subscribeScanCloud = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); + pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); + pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); + pnh.param("scan_normal_k", scanNormalK_, scanNormalK_); + if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k")) + { + ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". " + "The value is still used. Use \"scan_normal_k\" to avoid this warning."); + pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_); + } + pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_); + + NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); + NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); + NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); + NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_); + NODELET_INFO("RGBDIcpOdometry: scan_normal_k = %d", scanNormalK_); + NODELET_INFO("RGBDIcpOdometry: scan_normal_radius = %f", scanNormalRadius_); + + ros::NodeHandle rgb_nh(nh, "rgb"); + ros::NodeHandle depth_nh(nh, "depth"); + ros::NodeHandle rgb_pnh(pnh, "rgb"); + ros::NodeHandle depth_pnh(pnh, "depth"); + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", 1); + + std::string subscribedTopicsMsg; + if(subscribeScanCloud) + { + cloud_sub_.subscribe(nh, "scan_cloud", 1); + if(approxSync) + { + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + else + { + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", + getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str(), + cloud_sub_.getTopic().c_str()); + } + else + { + scan_sub_.subscribe(nh, "scan", 1); + if(approxSync) + { + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + else + { + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", + getName().c_str(), + approxSync?"approx":"exact", + image_mono_sub_.getTopic().c_str(), + image_depth_sub_.getTopic().c_str(), + info_sub_.getTopic().c_str(), + scan_sub_.getTopic().c_str()); + } + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2")); + } + + void callbackScan( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::LaserScanConstPtr& scanMsg) + { + sensor_msgs::PointCloud2ConstPtr cloudMsg; + callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); + } + + void callbackCloud( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + sensor_msgs::LaserScanConstPtr scanMsg; + callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); + } + + void callbackCommon( + const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& cloudMsg) + { + callbackCalled(); + if(!this->isPaused()) + { + if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || + !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " + "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", + image->encoding.c_str(), depth->encoding.c_str()); + return; + } + + // use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node + ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp; + if(scanMsg.get() != 0) + { + if(stamp < scanMsg->header.stamp) + { + stamp = scanMsg->header.stamp; + } + } + else if(cloudMsg.get() != 0) + { + if(stamp < cloudMsg->header.stamp) + { + stamp = cloudMsg->header.stamp; + } + } + + Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) + { + rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); + cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); + cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); + + cv::Mat scan; + Transform localScanTransform = Transform::getIdentity(); + int maxLaserScans = 0; + if(scanMsg.get() != 0) + { + // make sure the frame of the laser is updated too + localScanTransform = getTransform(this->frameId(), + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); + return; + } + + //transform in frameId_ frame + sensor_msgs::PointCloud2 scanOut; + laser_geometry::LaserProjection projection; + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(scanOut, *pclScan); + pclScan->is_dense = true; + + maxLaserScans = (int)scanMsg->ranges.size(); + if(pclScan->size()) + { + if(scanVoxelSize_ > 0.0f) + { + float pointsBeforeFiltering = (float)pclScan->size(); + pclScan = util3d::voxelize(pclScan, scanVoxelSize_); + float ratio = float(pclScan->size()) / pointsBeforeFiltering; + maxLaserScans = int(float(maxLaserScans) * ratio); + } + if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + { + //compute normals + pcl::PointCloud::Ptr normals; + if(scanVoxelSize_ > 0.0f) + { + normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); + } + else + { + normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_); + } + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + } + } + else if(cloudMsg.get() != 0) + { + bool containNormals = false; + if(scanVoxelSize_ == 0.0f) + { + for(unsigned int i=0; ifields.size(); ++i) + { + if(cloudMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + } + localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + if(localScanTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); + return; + } + + maxLaserScans = scanCloudMaxPoints_; + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + if(!pclScan->is_dense) + { + pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan); + } + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + if(!pclScan->is_dense) + { + pclScan = util3d::removeNaNFromPointCloud(pclScan); + } + + if(pclScan->size()) + { + if(scanVoxelSize_ > 0.0f) + { + float pointsBeforeFiltering = (float)pclScan->size(); + pclScan = util3d::voxelize(pclScan, scanVoxelSize_); + float ratio = float(pclScan->size()) / pointsBeforeFiltering; + maxLaserScans = int(float(maxLaserScans) * ratio); + } + if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + { + //compute normals + pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = util3d::laserScanFromPointCloud(*pclScanNormal); + } + else + { + scan = util3d::laserScanFromPointCloud(*pclScan); + } + } + } + } + + rtabmap::SensorData data( + LaserScan::backwardCompatibility(scan, + scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0, + scanMsg.get() != 0?scanMsg->range_max:0, + localScanTransform), + ptrImage->image, + ptrDepth->image, + rtabmapModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + } + } + +protected: + virtual void flushCallbacks() + { + // flush callbacks + if(approxScanSync_) + { + delete approxScanSync_; + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + if(exactScanSync_) + { + delete exactScanSync_; + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); + } + if(approxCloudSync_) + { + delete approxCloudSync_; + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + if(exactCloudSync_) + { + delete exactCloudSync_; + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); + } + } + +private: + image_transport::SubscriberFilter image_mono_sub_; + image_transport::SubscriberFilter image_depth_sub_; + message_filters::Subscriber info_sub_; + message_filters::Subscriber scan_sub_; + message_filters::Subscriber cloud_sub_; + typedef message_filters::sync_policies::ApproximateTime MyApproxScanSyncPolicy; + message_filters::Synchronizer * approxScanSync_; + typedef message_filters::sync_policies::ApproximateTime MyExactScanSyncPolicy; + message_filters::Synchronizer * exactScanSync_; + typedef message_filters::sync_policies::ApproximateTime MyApproxCloudSyncPolicy; + message_filters::Synchronizer * approxCloudSync_; + typedef message_filters::sync_policies::ApproximateTime MyExactCloudSyncPolicy; + message_filters::Synchronizer * exactCloudSync_; + int queueSize_; + int scanCloudMaxPoints_; + double scanVoxelSize_; + int scanNormalK_; + double scanNormalRadius_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet); + +} diff --git a/rtabmap_demo/src/nodelets/stereo_odometry.cpp b/rtabmap_demo/src/nodelets/stereo_odometry.cpp new file mode 100644 index 0000000..01dc13e --- /dev/null +++ b/rtabmap_demo/src/nodelets/stereo_odometry.cpp @@ -0,0 +1,347 @@ +/* +Copyright (c) 2010-2016, 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 "rtabmap_ros/OdometryROS.h" +#include "pluginlib/class_list_macros.h" +#include "nodelet/nodelet.h" + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include "rtabmap_ros/MsgConversion.h" + +#include +#include +#include +#include + +using namespace rtabmap; + +namespace rtabmap_ros +{ + +class StereoOdometry : public rtabmap_ros::OdometryROS +{ +public: + StereoOdometry() : + rtabmap_ros::OdometryROS(true, true, false), + approxSync_(0), + exactSync_(0), + queueSize_(5) + { + } + + virtual ~StereoOdometry() + { + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + } + +private: + virtual void onOdomInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + bool approxSync = false; + bool subscribeRGBD = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); + + NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); + NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_); + NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); + + std::string subscribedTopicsMsg; + if(subscribeRGBD) + { + rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); + + subscribedTopicsMsg = + uFormat("\n%s subscribed to:\n %s", + getName().c_str(), + rgbdSub_.getTopic().c_str()); + } + else + { + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + ros::NodeHandle left_pnh(pnh, "left"); + ros::NodeHandle right_pnh(pnh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); + image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); + + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); + cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + + + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str()); + } + this->startWarningThread(subscribedTopicsMsg, approxSync); + } + + virtual void updateParameters(ParametersMap & parameters) + { + //make sure we are using Reg/Strategy=0 + ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); + if(iter != parameters.end() && iter->second.compare("0") != 0) + { + ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); + } + uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); + } + + void callback( + const sensor_msgs::ImageConstPtr& imageRectLeft, + const sensor_msgs::ImageConstPtr& imageRectRight, + const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, + const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) + { + callbackCalled(); + if(!this->isPaused()) + { + if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); + return; + } + + ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; + + Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + ros::WallTime time = ros::WallTime::now(); + + int quality = -1; + if(imageRectLeft->data.size() && imageRectRight->data.size()) + { + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform); + if(stereoModel.baseline() <= 0) + { + NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " + "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); + return; + } + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + NODELET_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + + cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); + cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); + + UTimer stepTimer; + // + UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); + rtabmap::SensorData data( + ptrImageLeft->image, + ptrImageRight->image, + stereoModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + else + { + NODELET_WARN("Odom: input images empty?!?"); + } + } + } + + void callbackRGBD( + const rtabmap_ros::RGBDImageConstPtr& image) + { + callbackCalled(); + if(!this->isPaused()) + { + cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight; + rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight); + + if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || + !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) + { + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); + return; + } + + ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; + + Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); + if(localTransform.isNull()) + { + return; + } + + ros::WallTime time = ros::WallTime::now(); + + int quality = -1; + if(!imageRectLeft->image.empty() && !imageRectRight->image.empty()) + { + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform); + if(stereoModel.baseline() <= 0) + { + NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " + "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); + return; + } + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + NODELET_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + + cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8"); + cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8"); + + UTimer stepTimer; + // + UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); + rtabmap::SensorData data( + ptrImageLeft->image, + ptrImageRight->image, + stereoModel, + 0, + rtabmap_ros::timestampFromROS(stamp)); + + this->processData(data, stamp); + } + else + { + NODELET_WARN("Odom: input images empty?!?"); + } + } + } + +protected: + virtual void flushCallbacks() + { + //flush callbacks + if(approxSync_) + { + delete approxSync_; + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + if(exactSync_) + { + delete exactSync_; + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); + } + } + +private: + image_transport::SubscriberFilter imageRectLeft_; + image_transport::SubscriberFilter imageRectRight_; + message_filters::Subscriber cameraInfoLeft_; + message_filters::Subscriber cameraInfoRight_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + ros::Subscriber rgbdSub_; + int queueSize_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet); + +} + diff --git a/rtabmap_demo/src/nodelets/stereo_throttle.cpp b/rtabmap_demo/src/nodelets/stereo_throttle.cpp new file mode 100644 index 0000000..d5db504 --- /dev/null +++ b/rtabmap_demo/src/nodelets/stereo_throttle.cpp @@ -0,0 +1,246 @@ +/* +Copyright (c) 2010-2016, 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 "ros/ros.h" +#include "pluginlib/class_list_macros.h" +#include "nodelet/nodelet.h" + +#include +#include +#include + +#include +#include + +#include + +#include + +#include + +namespace rtabmap_ros +{ + +class StereoThrottleNodelet : public nodelet::Nodelet +{ +public: + //Constructor + StereoThrottleNodelet(): + rate_(0), + approxSync_(0), + exactSync_(0), + decimation_(1) + { + } + + virtual ~StereoThrottleNodelet() + { + if(approxSync_) + { + delete approxSync_; + } + if(exactSync_) + { + delete exactSync_; + } + } + +private: + ros::Time last_update_; + double rate_; + virtual void onInit() + { + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& pnh = getPrivateNodeHandle(); + + ros::NodeHandle left_nh(nh, "left"); + ros::NodeHandle right_nh(nh, "right"); + ros::NodeHandle left_pnh(pnh, "left"); + ros::NodeHandle right_pnh(pnh, "right"); + image_transport::ImageTransport left_it(left_nh); + image_transport::ImageTransport right_it(right_nh); + image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); + image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); + + int queueSize = 5; + bool approxSync = false; + pnh.param("approx_sync", approxSync, approxSync); + pnh.param("rate", rate_, rate_); + pnh.param("queue_size", queueSize, queueSize); + pnh.param("decimation", decimation_, decimation_); + ROS_ASSERT(decimation_ >= 1); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); + + if(approxSync) + { + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4)); + } + else + { + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4)); + } + + imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); + imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); + cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + + imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1); + imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1); + infoLeftPub_ = left_nh.advertise(left_nh.resolveName("camera_info")+"_throttle", 1); + infoRightPub_ = right_nh.advertise(right_nh.resolveName("camera_info")+"_throttle", 1); + }; + + void callback(const sensor_msgs::ImageConstPtr& imageLeft, + const sensor_msgs::ImageConstPtr& imageRight, + const sensor_msgs::CameraInfoConstPtr& camInfoLeft, + const sensor_msgs::CameraInfoConstPtr& camInfoRight) + { + if (rate_ > 0.0) + { + NODELET_DEBUG("update set to %f", rate_); + if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) + { + NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); + return; + } + } + else + NODELET_DEBUG("rate unset continuing"); + + last_update_ = ros::Time::now(); + + if(imageLeftPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft); + cv_bridge::CvImage out; + out.header = imagePtr->header; + out.encoding = imagePtr->encoding; + out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); + imageLeftPub_.publish(out.toImageMsg()); + } + else + { + imageLeftPub_.publish(imageLeft); + } + } + if(imageRightPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight); + cv_bridge::CvImage out; + out.header = imagePtr->header; + out.encoding = imagePtr->encoding; + out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); + imageRightPub_.publish(out.toImageMsg()); + } + else + { + imageRightPub_.publish(imageRight); + } + } + if(infoLeftPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + sensor_msgs::CameraInfo info = *camInfoLeft; + info.height /= decimation_; + info.width /= decimation_; + info.roi.height /= decimation_; + info.roi.width /= decimation_; + info.K[2]/=float(decimation_); // cx + info.K[5]/=float(decimation_); // cy + info.K[0]/=float(decimation_); // fx + info.K[4]/=float(decimation_); // fy + info.P[2]/=float(decimation_); // cx + info.P[6]/=float(decimation_); // cy + info.P[0]/=float(decimation_); // fx + info.P[5]/=float(decimation_); // fy + info.P[3]/=float(decimation_); // Tx + infoLeftPub_.publish(info); + } + else + { + infoLeftPub_.publish(camInfoLeft); + } + } + if(infoRightPub_.getNumSubscribers()) + { + if(decimation_ > 1) + { + sensor_msgs::CameraInfo info = *camInfoRight; + info.height /= decimation_; + info.width /= decimation_; + info.roi.height /= decimation_; + info.roi.width /= decimation_; + info.K[2]/=float(decimation_); // cx + info.K[5]/=float(decimation_); // cy + info.K[0]/=float(decimation_); // fx + info.K[4]/=float(decimation_); // fy + info.P[2]/=float(decimation_); // cx + info.P[6]/=float(decimation_); // cy + info.P[0]/=float(decimation_); // fx + info.P[5]/=float(decimation_); // fy + info.P[3]/=float(decimation_); // Tx + infoRightPub_.publish(info); + } + else + { + infoRightPub_.publish(camInfoRight); + } + } + } + + image_transport::Publisher imageLeftPub_; + image_transport::Publisher imageRightPub_; + ros::Publisher infoLeftPub_; + ros::Publisher infoRightPub_; + + image_transport::SubscriberFilter imageLeft_; + image_transport::SubscriberFilter imageRight_; + message_filters::Subscriber cameraInfoLeft_; + message_filters::Subscriber cameraInfoRight_; + + typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; + message_filters::Synchronizer * approxSync_; + typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; + message_filters::Synchronizer * exactSync_; + + int decimation_; + +}; + + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoThrottleNodelet, nodelet::Nodelet); +} diff --git a/rtabmap_demo/src/nodelets/undistort_depth.cpp b/rtabmap_demo/src/nodelets/undistort_depth.cpp new file mode 100644 index 0000000..01a5665 --- /dev/null +++ b/rtabmap_demo/src/nodelets/undistort_depth.cpp @@ -0,0 +1,121 @@ +/* +Copyright (c) 2010-2016, 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 + +#include "rtabmap/core/clams/discrete_depth_distortion_model.h" +#include "rtabmap/utilite/UConversion.h" + +namespace rtabmap_ros +{ + +class UndistortDepth : public nodelet::Nodelet +{ +public: + UndistortDepth() + {} + + virtual ~UndistortDepth() + { + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + std::string modelPath; + pnh.param("model", modelPath, modelPath); + + if(modelPath.empty()) + { + NODELET_ERROR("undistort_depth: \"model\" parameter should be set!"); + } + + model_.load(modelPath); + if(!model_.isValid()) + { + NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str()); + } + else + { + image_transport::ImageTransport it(nh); + sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this); + pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1); + } + } + + void callback(const sensor_msgs::ImageConstPtr& depth) + { + if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) + { + NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); + return; + } + + if(pub_.getNumSubscribers()) + { + if(depth->width == model_.getWidth() && depth->width == model_.getWidth()) + { + cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth); + model_.undistort(imageDepthPtr->image); + pub_.publish(imageDepthPtr->toImageMsg()); + } + else + { + NODELET_ERROR("Input depth image size (%dx%d) and distortion model " + "size (%dx%d) don't match! Cannot undistort image.", + depth->width, depth->height, + model_.getWidth(), model_.getHeight()); + } + } + } + +private: + clams::DiscreteDepthDistortionModel model_; + image_transport::Publisher pub_; + image_transport::Subscriber sub_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap_ros::UndistortDepth, nodelet::Nodelet); +} + diff --git a/rtabmap_demo/src/rviz/InfoDisplay.cpp b/rtabmap_demo/src/rviz/InfoDisplay.cpp new file mode 100644 index 0000000..e174f76 --- /dev/null +++ b/rtabmap_demo/src/rviz/InfoDisplay.cpp @@ -0,0 +1,131 @@ +/* +Copyright (c) 2010-2016, 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 "InfoDisplay.h" +#include "rtabmap_ros/MsgConversion.h" + +namespace rtabmap_ros +{ + +InfoDisplay::InfoDisplay() + : spinner_(1, &cbqueue_), + globalCount_(0), + localCount_(0) +{ + update_nh_.setCallbackQueue( &cbqueue_ ); +} + +InfoDisplay::~InfoDisplay() +{ + spinner_.stop(); +} + +void InfoDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + this->setStatusStd(rviz::StatusProperty::Ok, "Info", ""); + this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); + this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0"); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0"); + + spinner_.start(); +} + +void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg ) +{ + { + boost::mutex::scoped_lock lock(info_mutex_); + if(msg->loopClosureId) + { + info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId); + globalCount_ += 1; + } + else if(msg->proximityDetectionId) + { + info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId); + localCount_ += 1; + } + else + { + info_ = ""; + } + loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform); + + rtabmap::Statistics stat; + rtabmap_ros::infoFromROS(*msg, stat); + statistics_ = stat.data(); + } + + + this->emitTimeSignal(msg->header.stamp); +} + +void InfoDisplay::update( float wall_dt, float ros_dt ) +{ + { + boost::mutex::scoped_lock lock(info_mutex_); + this->setStatusStd(rviz::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString()); + if(loopTransform_.isNull()) + { + this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); + this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); + } + else + { + float x,y,z, roll,pitch,yaw; + loopTransform_.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString()); + } + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString()); + + for(std::map::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter) + { + this->setStatus(rviz::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second)); + } + } +} + +void InfoDisplay::reset() +{ + MFDClass::reset(); + { + boost::mutex::scoped_lock lock(info_mutex_); + info_.clear(); + globalCount_ = 0; + localCount_ = 0; + statistics_.clear(); + } +} + +} // namespace rtabmap_ros + +#include +PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/InfoDisplay.h b/rtabmap_demo/src/rviz/InfoDisplay.h new file mode 100644 index 0000000..7654f2a --- /dev/null +++ b/rtabmap_demo/src/rviz/InfoDisplay.h @@ -0,0 +1,70 @@ +/* +Copyright (c) 2010-2016, 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 INFO_DISPLAY_H +#define INFO_DISPLAY_H + +#include + +#include +#include + +namespace rtabmap_ros +{ + +class InfoDisplay: public rviz::MessageFilterDisplay +{ +Q_OBJECT +public: + InfoDisplay(); + virtual ~InfoDisplay(); + + virtual void reset(); + virtual void update( float wall_dt, float ros_dt ); + +protected: + /** @brief Do initialization. Overridden from MessageFilterDisplay. */ + virtual void onInitialize(); + + /** @brief Process a single message. Overridden from MessageFilterDisplay. */ + virtual void processMessage( const rtabmap_ros::InfoConstPtr& cloud ); + +private: + ros::AsyncSpinner spinner_; + ros::CallbackQueue cbqueue_; + + QString info_; + int globalCount_; + int localCount_; + std::map statistics_; + rtabmap::Transform loopTransform_; + boost::mutex info_mutex_; +}; + +} // namespace rtabmap_ros + +#endif diff --git a/rtabmap_demo/src/rviz/MapCloudDisplay.cpp b/rtabmap_demo/src/rviz/MapCloudDisplay.cpp new file mode 100644 index 0000000..e94e616 --- /dev/null +++ b/rtabmap_demo/src/rviz/MapCloudDisplay.cpp @@ -0,0 +1,901 @@ +/* +Copyright (c) 2010-2016, 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 +#include +#include +#include "rviz/properties/bool_property.h" +#include "rviz/properties/enum_property.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/vector_property.h" + +#include + +#include "MapCloudDisplay.h" +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace rtabmap_ros +{ + + +MapCloudDisplay::CloudInfo::CloudInfo() : + manager_(0), + pose_(rtabmap::Transform::getIdentity()), + id_(0), + scene_node_(0) +{} + +MapCloudDisplay::CloudInfo::~CloudInfo() +{ + clear(); +} + +void MapCloudDisplay::CloudInfo::clear() +{ + if ( scene_node_ ) + { + manager_->destroySceneNode( scene_node_ ); + scene_node_=0; + } +} + +MapCloudDisplay::MapCloudDisplay() + : spinner_(1, &cbqueue_), + new_xyz_transformer_(false), + new_color_transformer_(false), + needs_retransform_(false), + transformer_class_loader_(NULL) +{ + //QIcon icon; + //this->setIcon(icon); + + style_property_ = new rviz::EnumProperty( "Style", "Flat Squares", + "Rendering mode to use, in order of computational complexity.", + this, SLOT( updateStyle() ), this ); + style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS ); + style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES ); + style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES ); + style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES ); + style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES ); + + point_world_size_property_ = new rviz::FloatProperty( "Size (m)", 0.01, + "Point size in meters.", + this, SLOT( updateBillboardSize() ), this ); + point_world_size_property_->setMin( 0.0001 ); + + point_pixel_size_property_ = new rviz::FloatProperty( "Size (Pixels)", 3, + "Point size in pixels.", + this, SLOT( updateBillboardSize() ), this ); + point_pixel_size_property_->setMin( 1 ); + + alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0, + "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.", + this, SLOT( updateAlpha() ), this ); + alpha_property_->setMin( 0 ); + alpha_property_->setMax( 1 ); + + xyz_transformer_property_ = new rviz::EnumProperty( "Position Transformer", "", + "Set the transformer to use to set the position of the points.", + this, SLOT( updateXyzTransformer() ), this ); + connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )), + this, SLOT( setXyzTransformerOptions( EnumProperty* ))); + + color_transformer_property_ = new rviz::EnumProperty( "Color Transformer", "", + "Set the transformer to use to set the color of the points.", + this, SLOT( updateColorTransformer() ), this ); + connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )), + this, SLOT( setColorTransformerOptions( EnumProperty* ))); + + cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4, + "Decimation of the input RGB and depth images before creating the cloud.", + this, SLOT( updateCloudParameters() ), this ); + cloud_decimation_->setMin( 1 ); + cloud_decimation_->setMax( 16 ); + + cloud_max_depth_ = new rviz::FloatProperty( "Cloud max depth (m)", 4.0f, + "Maximum depth of the generated clouds.", + this, SLOT( updateCloudParameters() ), this ); + cloud_max_depth_->setMin( 0.0f ); + cloud_max_depth_->setMax( 999.0f ); + + cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f, + "Minimum depth of the generated clouds.", + this, SLOT( updateCloudParameters() ), this ); + cloud_min_depth_->setMin( 0.0f ); + cloud_min_depth_->setMax( 999.0f ); + + cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f, + "Voxel size of the generated clouds.", + this, SLOT( updateCloudParameters() ), this ); + cloud_voxel_size_->setMin( 0.0f ); + cloud_voxel_size_->setMax( 1.0f ); + + cloud_filter_floor_height_ = new rviz::FloatProperty( "Filter floor (m)", 0.0f, + "Filter the floor up to maximum height set here " + "(only appropriate for 2D mapping).", + this, SLOT( updateCloudParameters() ), this ); + cloud_filter_floor_height_->setMin( 0.0f ); + cloud_filter_floor_height_->setMax( 999.0f ); + + cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f, + "Filter the ceiling at the specified height set here " + "(only appropriate for 2D mapping).", + this, SLOT( updateCloudParameters() ), this ); + cloud_filter_ceiling_height_->setMin( 0.0f ); + cloud_filter_ceiling_height_->setMax( 999.0f ); + + node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.0f, + "(Disabled=0) Only keep one node in the specified radius.", + this, SLOT( updateCloudParameters() ), this ); + node_filtering_radius_->setMin( 0.0f ); + node_filtering_radius_->setMax( 10.0f ); + + node_filtering_angle_ = new rviz::FloatProperty( "Node filtering angle (degrees)", 30.0f, + "(Disabled=0) Only keep one node in the specified angle in the filtering radius.", + this, SLOT( updateCloudParameters() ), this ); + node_filtering_angle_->setMin( 0.0f ); + node_filtering_angle_->setMax( 359.0f ); + + download_map_ = new rviz::BoolProperty( "Download map", false, + "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.", + this, SLOT( downloadMap() ), this ); + + download_graph_ = new rviz::BoolProperty( "Download graph", false, + "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.", + this, SLOT( downloadGraph() ), this ); + + // PointCloudCommon sets up a callback queue with a thread for each + // instance. Use that for processing incoming messages. + update_nh_.setCallbackQueue( &cbqueue_ ); +} + +MapCloudDisplay::~MapCloudDisplay() +{ + if ( transformer_class_loader_ ) + { + delete transformer_class_loader_; + } + + spinner_.stop(); +} + +void MapCloudDisplay::loadTransformers() +{ + std::vector classes = transformer_class_loader_->getDeclaredClasses(); + std::vector::iterator ci; + + for( ci = classes.begin(); ci != classes.end(); ci++ ) + { + const std::string& lookup_name = *ci; + std::string name = transformer_class_loader_->getName( lookup_name ); + + if( transformers_.count( name ) > 0 ) + { + ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() ); + continue; + } + + rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name )); + trans->init(); + connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() )); + + TransformerInfo info; + info.transformer = trans; + info.readable_name = name; + info.lookup_name = lookup_name; + + info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_XYZ, info.xyz_props ); + setPropertiesHidden( info.xyz_props, true ); + + info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props ); + setPropertiesHidden( info.color_props, true ); + + transformers_[ name ] = info; + } +} + +void MapCloudDisplay::onInitialize() +{ + MFDClass::onInitialize(); + + transformer_class_loader_ = new pluginlib::ClassLoader( "rviz", "rviz::PointCloudTransformer" ); + loadTransformers(); + + updateStyle(); + updateBillboardSize(); + updateAlpha(); + + spinner_.start(); +} + +void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg ) +{ + processMapData(*msg); + + this->emitTimeSignal(msg->header.stamp); +} + +void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) +{ + std::map poses; + for(unsigned int i=0; i::Ptr cloud; + pcl::IndicesPtr validIndices(new std::vector); + cloud = rtabmap::util3d::cloudRGBFromSensorData( + s.sensorData(), + cloud_decimation_->getInt(), + cloud_max_depth_->getFloat(), + cloud_min_depth_->getFloat(), + validIndices.get()); + + if(cloud_voxel_size_->getFloat()) + { + cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); + } + + if(cloud->size()) + { + if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f) + { + // convert in /odom frame + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose()); + cloud = rtabmap::util3d::passThrough(cloud, "z", + cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f, + cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); + // convert back in /base_link frame + cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); + } + + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*cloud, *cloudMsg); + cloudMsg->header = map.header; + + CloudInfoPtr info(new CloudInfo); + info->message_ = cloudMsg; + info->pose_ = rtabmap::Transform::getIdentity(); + info->id_ = id; + + if (transformCloud(info, true)) + { + boost::mutex::scoped_lock lock(new_clouds_mutex_); + new_cloud_infos_.erase(id); + new_cloud_infos_.insert(std::make_pair(id, info)); + } + } + } + } + } + + // Update graph + if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) + { + poses = rtabmap::graph::radiusPosesFiltering(poses, + node_filtering_radius_->getFloat(), + node_filtering_angle_->getFloat()*CV_PI/180.0); + } + + { + boost::mutex::scoped_lock lock(current_map_mutex_); + current_map_ = poses; + } +} + +void MapCloudDisplay::setPropertiesHidden( const QList& props, bool hide ) +{ + for( int i = 0; i < props.size(); i++ ) + { + props[ i ]->setHidden( hide ); + } +} + +void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ) +{ + std::string xyz_name = xyz_transformer_property_->getStdString(); + std::string color_name = color_transformer_property_->getStdString(); + + xyz_transformer_property_->clearOptions(); + color_transformer_property_->clearOptions(); + + // Get the channels that we could potentially render + typedef std::set > S_string; + S_string valid_xyz, valid_color; + bool cur_xyz_valid = false; + bool cur_color_valid = false; + bool has_rgb_transformer = false; + M_TransformerInfo::iterator trans_it = transformers_.begin(); + M_TransformerInfo::iterator trans_end = transformers_.end(); + for(;trans_it != trans_end; ++trans_it) + { + const std::string& name = trans_it->first; + const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer; + uint32_t mask = trans->supports(cloud); + if (mask & rviz::PointCloudTransformer::Support_XYZ) + { + valid_xyz.insert(std::make_pair(trans->score(cloud), name)); + if (name == xyz_name) + { + cur_xyz_valid = true; + } + xyz_transformer_property_->addOptionStd( name ); + } + + if (mask & rviz::PointCloudTransformer::Support_Color) + { + valid_color.insert(std::make_pair(trans->score(cloud), name)); + if (name == color_name) + { + cur_color_valid = true; + } + if (name == "RGB8") + { + has_rgb_transformer = true; + } + color_transformer_property_->addOptionStd( name ); + } + } + + if( !cur_xyz_valid ) + { + if( !valid_xyz.empty() ) + { + xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second ); + } + } + + if( !cur_color_valid ) + { + if( !valid_color.empty() ) + { + if (has_rgb_transformer) + { + color_transformer_property_->setStringStd( "RGB8" ); + } + else + { + color_transformer_property_->setStringStd( valid_color.rbegin()->second ); + } + } + } +} + +void MapCloudDisplay::updateAlpha() +{ + for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) + { + it->second->cloud_->setAlpha( alpha_property_->getFloat() ); + } +} + +void MapCloudDisplay::updateStyle() +{ + rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); + if( mode == rviz::PointCloud::RM_POINTS ) + { + point_world_size_property_->hide(); + point_pixel_size_property_->show(); + } + else + { + point_world_size_property_->show(); + point_pixel_size_property_->hide(); + } + for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) + { + it->second->cloud_->setRenderMode( mode ); + } + updateBillboardSize(); +} + +void MapCloudDisplay::updateBillboardSize() +{ + rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); + float size; + if( mode == rviz::PointCloud::RM_POINTS ) + { + size = point_pixel_size_property_->getFloat(); + } + else + { + size = point_world_size_property_->getFloat(); + } + for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) + { + it->second->cloud_->setDimensions( size, size, size ); + } + context_->queueRender(); +} + +void MapCloudDisplay::updateCloudParameters() +{ + // do nothing... only take effect on next generated clouds +} + +void MapCloudDisplay::downloadMap() +{ + if(download_map_->getBool()) + { + rtabmap_ros::GetMap getMapSrv; + getMapSrv.request.global = true; + getMapSrv.request.optimized = true; + getMapSrv.request.graphOnly = false; + ros::NodeHandle nh; + QMessageBox * messageBox = new QMessageBox( + QMessageBox::NoIcon, + tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()), + tr("Downloading the map... please wait (rviz could become gray!)"), + QMessageBox::NoButton); + messageBox->setAttribute(Qt::WA_DeleteOnClose, true); + messageBox->show(); + QApplication::processEvents(); + uSleep(100); // hack make sure the text in the QMessageBox is shown... + QApplication::processEvents(); + if(!ros::service::call("rtabmap/get_map_data", getMapSrv)) + { + ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. " + "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " + "to \"get_map_data\" in the launch " + "file like: .", + nh.resolveName("rtabmap/get_map_data").c_str()); + messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. " + "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " + "to \"get_map_data\" in the launch " + "file like: ."). + arg(nh.resolveName("rtabmap/get_map_data").c_str())); + } + else + { + messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...") + .arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); + QApplication::processEvents(); + this->reset(); + processMapData(getMapSrv.response.data); + messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!") + .arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); + + QTimer::singleShot(1000, messageBox, SLOT(close())); + } + download_map_->blockSignals(true); + download_map_->setBool(false); + download_map_->blockSignals(false); + } + else + { + // just stay true if double-clicked on DownloadMap property, let the + // first process above finishes + download_map_->blockSignals(true); + download_map_->setBool(true); + download_map_->blockSignals(false); + } +} + +void MapCloudDisplay::downloadGraph() +{ + if(download_graph_->getBool()) + { + rtabmap_ros::GetMap getMapSrv; + getMapSrv.request.global = true; + getMapSrv.request.optimized = true; + getMapSrv.request.graphOnly = true; + ros::NodeHandle nh; + QMessageBox * messageBox = new QMessageBox( + QMessageBox::NoIcon, + tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()), + tr("Downloading the graph... please wait (rviz could become gray!)"), + QMessageBox::NoButton); + messageBox->setAttribute(Qt::WA_DeleteOnClose, true); + messageBox->show(); + QApplication::processEvents(); + uSleep(100); // hack make sure the text in the QMessageBox is shown... + QApplication::processEvents(); + if(!ros::service::call("rtabmap/get_map_data", getMapSrv)) + { + ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. " + "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " + "to \"get_map_data\" in the launch " + "file like: .", + nh.resolveName("rtabmap/get_map_data").c_str()); + messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. " + "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " + "to \"get_map_data\" in the launch " + "file like: ."). + arg(nh.resolveName("rtabmap/get_map_data").c_str())); + } + else + { + messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size())); + QApplication::processEvents(); + processMapData(getMapSrv.response.data); + messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size())); + + QTimer::singleShot(1000, messageBox, SLOT(close())); + } + download_graph_->blockSignals(true); + download_graph_->setBool(false); + download_graph_->blockSignals(false); + } + else + { + // just stay true if double-clicked on DownloadGraph property, let the + // first process above finishes + download_graph_->blockSignals(true); + download_graph_->setBool(true); + download_graph_->blockSignals(false); + } +} + +void MapCloudDisplay::causeRetransform() +{ + needs_retransform_ = true; +} + +void MapCloudDisplay::update( float wall_dt, float ros_dt ) +{ + rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); + + if (needs_retransform_) + { + retransform(); + needs_retransform_ = false; + } + + { + boost::mutex::scoped_lock lock(new_clouds_mutex_); + if( !new_cloud_infos_.empty() ) + { + float size; + if( mode == rviz::PointCloud::RM_POINTS ) { + size = point_pixel_size_property_->getFloat(); + } else { + size = point_world_size_property_->getFloat(); + } + + std::map::iterator it = new_cloud_infos_.begin(); + std::map::iterator end = new_cloud_infos_.end(); + for (; it != end; ++it) + { + CloudInfoPtr cloud_info = it->second; + + cloud_info->cloud_.reset( new rviz::PointCloud() ); + cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() ); + cloud_info->cloud_->setRenderMode( mode ); + cloud_info->cloud_->setAlpha( alpha_property_->getFloat() ); + cloud_info->cloud_->setDimensions( size, size, size ); + cloud_info->cloud_->setAutoSize(false); + + cloud_info->manager_ = context_->getSceneManager(); + + cloud_info->scene_node_ = scene_node_->createChildSceneNode(); + + cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() ); + cloud_info->scene_node_->setVisible(false); + + cloud_infos_.erase(it->first); + cloud_infos_.insert(*it); + } + + new_cloud_infos_.clear(); + } + } + + { + boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ ); + + if( lock.owns_lock() ) + { + if( new_xyz_transformer_ || new_color_transformer_ ) + { + M_TransformerInfo::iterator it = transformers_.begin(); + M_TransformerInfo::iterator end = transformers_.end(); + for (; it != end; ++it) + { + const std::string& name = it->first; + TransformerInfo& info = it->second; + + setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() ); + setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() ); + } + } + } + + new_xyz_transformer_ = false; + new_color_transformer_ = false; + } + + int totalPoints = 0; + int totalNodesShown = 0; + { + // update poses + boost::mutex::scoped_lock lock(current_map_mutex_); + if(!current_map_.empty()) + { + for (std::map::iterator it=current_map_.begin(); it != current_map_.end(); ++it) + { + std::map::iterator cloudInfoIt = cloud_infos_.find(it->first); + if(cloudInfoIt != cloud_infos_.end()) + { + totalPoints += cloudInfoIt->second->transformed_points_.size(); + cloudInfoIt->second->pose_ = it->second; + Ogre::Vector3 framePosition; + Ogre::Quaternion frameOrientation; + if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation)) + { + // Multiply frame with pose + Ogre::Matrix4 frameTransform; + frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation); + const rtabmap::Transform & p = cloudInfoIt->second->pose_; + Ogre::Matrix4 pose(p[0], p[1], p[2], p[3], + p[4], p[5], p[6], p[7], + p[8], p[9], p[10], p[11], + 0, 0, 0, 1); + frameTransform = frameTransform * pose; + Ogre::Vector3 posePosition = frameTransform.getTrans(); + Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion(); + poseOrientation.normalise(); + + cloudInfoIt->second->scene_node_->setPosition(posePosition); + cloudInfoIt->second->scene_node_->setOrientation(poseOrientation); + cloudInfoIt->second->scene_node_->setVisible(true); + ++totalNodesShown; + } + else + { + ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first); + } + + } + } + //hide not used clouds + for(std::map::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter) + { + if(current_map_.find(iter->first) == current_map_.end()) + { + iter->second->scene_node_->setVisible(false); + } + } + } + } + + this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString()); +} + +void MapCloudDisplay::reset() +{ + { + boost::mutex::scoped_lock lock(new_clouds_mutex_); + cloud_infos_.clear(); + new_cloud_infos_.clear(); + } + { + boost::mutex::scoped_lock lock(current_map_mutex_); + current_map_.clear(); + } + MFDClass::reset(); +} + +void MapCloudDisplay::updateXyzTransformer() +{ + boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); + if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 ) + { + return; + } + new_xyz_transformer_ = true; + causeRetransform(); +} + +void MapCloudDisplay::updateColorTransformer() +{ + boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); + if( transformers_.count( color_transformer_property_->getStdString() ) == 0 ) + { + return; + } + new_color_transformer_ = true; + causeRetransform(); +} + +void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop ) +{ + fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ ); +} + +void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop ) +{ + fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color ); +} + +void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask ) +{ + prop->clearOptions(); + + if (cloud_infos_.empty()) + { + return; + } + + boost::recursive_mutex::scoped_lock tlock(transformers_mutex_); + + const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_; + + M_TransformerInfo::iterator it = transformers_.begin(); + M_TransformerInfo::iterator end = transformers_.end(); + for (; it != end; ++it) + { + const rviz::PointCloudTransformerPtr& trans = it->second.transformer; + if ((trans->supports(msg) & mask) == mask) + { + prop->addOption( QString::fromStdString( it->first )); + } + } +} + +rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) +{ + boost::recursive_mutex::scoped_lock lock( transformers_mutex_); + M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() ); + if( it != transformers_.end() ) + { + const rviz::PointCloudTransformerPtr& trans = it->second.transformer; + if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ ) + { + return trans; + } + } + + return rviz::PointCloudTransformerPtr(); +} + +rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) +{ + boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); + M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() ); + if( it != transformers_.end() ) + { + const rviz::PointCloudTransformerPtr& trans = it->second.transformer; + if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color ) + { + return trans; + } + } + + return rviz::PointCloudTransformerPtr(); +} + + +void MapCloudDisplay::retransform() +{ + boost::recursive_mutex::scoped_lock lock(transformers_mutex_); + + for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) + { + const CloudInfoPtr& cloud_info = it->second; + transformCloud(cloud_info, false); + cloud_info->cloud_->clear(); + cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size()); + } +} + +bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers) +{ + rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_; + cloud_points.clear(); + + size_t size = cloud_info->message_->width * cloud_info->message_->height; + rviz::PointCloud::Point default_pt; + default_pt.color = Ogre::ColourValue(1, 1, 1); + default_pt.position = Ogre::Vector3::ZERO; + cloud_points.resize(size, default_pt); + + { + boost::recursive_mutex::scoped_lock lock(transformers_mutex_); + if( update_transformers ) + { + updateTransformers( cloud_info->message_ ); + } + rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_); + rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_); + + if (!xyz_trans) + { + std::stringstream ss; + ss << "No position transformer available for cloud"; + this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str()); + return false; + } + + if (!color_trans) + { + std::stringstream ss; + ss << "No color transformer available for cloud"; + this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str()); + return false; + } + + xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points); + color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points); + } + + for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point) + { + if (!rviz::validateFloats(cloud_point->position)) + { + cloud_point->position.x = 999999.0f; + cloud_point->position.y = 999999.0f; + cloud_point->position.z = 999999.0f; + } + } + + return true; +} + +} // namespace rtabmap + +#include +PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapCloudDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/MapCloudDisplay.h b/rtabmap_demo/src/rviz/MapCloudDisplay.h new file mode 100644 index 0000000..938bef9 --- /dev/null +++ b/rtabmap_demo/src/rviz/MapCloudDisplay.h @@ -0,0 +1,196 @@ +/* +Copyright (c) 2010-2016, 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 MAP_CLOUD_DISPLAY_H +#define MAP_CLOUD_DISPLAY_H + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#endif + +namespace rviz { +class IntProperty; +class BoolProperty; +class EnumProperty; +class FloatProperty; +class PointCloudTransformer; +typedef boost::shared_ptr PointCloudTransformerPtr; +typedef std::vector V_string; +} + +using namespace rviz; + +namespace rtabmap_ros +{ + +class PointCloudCommon; + +/** + * \class MapCloudDisplay + * \brief Displays point clouds from rtabmap::MapData + * + * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity. + * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b + * all being 8 bits. + */ +class MapCloudDisplay: public rviz::MessageFilterDisplay +{ +Q_OBJECT +public: + struct CloudInfo + { + CloudInfo(); + ~CloudInfo(); + + // clear the point cloud, but keep selection handler around + void clear(); + + Ogre::SceneManager *manager_; + + sensor_msgs::PointCloud2ConstPtr message_; + rtabmap::Transform pose_; + int id_; + + Ogre::SceneNode *scene_node_; + boost::shared_ptr cloud_; + + std::vector transformed_points_; + }; + typedef boost::shared_ptr CloudInfoPtr; + + MapCloudDisplay(); + virtual ~MapCloudDisplay(); + + virtual void reset(); + virtual void update( float wall_dt, float ros_dt ); + + rviz::FloatProperty* point_world_size_property_; + rviz::FloatProperty* point_pixel_size_property_; + rviz::FloatProperty* alpha_property_; + rviz::EnumProperty* xyz_transformer_property_; + rviz::EnumProperty* color_transformer_property_; + rviz::EnumProperty* style_property_; + rviz::IntProperty* cloud_decimation_; + rviz::FloatProperty* cloud_max_depth_; + rviz::FloatProperty* cloud_min_depth_; + rviz::FloatProperty* cloud_voxel_size_; + rviz::FloatProperty* cloud_filter_floor_height_; + rviz::FloatProperty* cloud_filter_ceiling_height_; + rviz::FloatProperty* node_filtering_radius_; + rviz::FloatProperty* node_filtering_angle_; + rviz::BoolProperty* download_map_; + rviz::BoolProperty* download_graph_; + +public Q_SLOTS: + void causeRetransform(); + +private Q_SLOTS: + void updateStyle(); + void updateBillboardSize(); + void updateAlpha(); + void updateXyzTransformer(); + void updateColorTransformer(); + void setXyzTransformerOptions( EnumProperty* prop ); + void setColorTransformerOptions( EnumProperty* prop ); + void updateCloudParameters(); + void downloadMap(); + void downloadGraph(); + +protected: + /** @brief Do initialization. Overridden from MessageFilterDisplay. */ + virtual void onInitialize(); + + /** @brief Process a single message. Overridden from MessageFilterDisplay. */ + virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud ); + +private: + void processMapData(const rtabmap_ros::MapData& map); + + /** + * \brief Transforms the cloud into the correct frame, and sets up our renderable cloud + */ + bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers); + + rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); + rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); + void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ); + void retransform(); + + void loadTransformers(); + + void setPropertiesHidden( const QList& props, bool hide ); + void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask ); + +private: + ros::AsyncSpinner spinner_; + ros::CallbackQueue cbqueue_; + + std::map cloud_infos_; + + std::map new_cloud_infos_; + boost::mutex new_clouds_mutex_; + + std::map current_map_; + boost::mutex current_map_mutex_; + + struct TransformerInfo + { + rviz::PointCloudTransformerPtr transformer; + QList xyz_props; + QList color_props; + + std::string readable_name; + std::string lookup_name; + }; + typedef std::map M_TransformerInfo; + + boost::recursive_mutex transformers_mutex_; + M_TransformerInfo transformers_; + bool new_xyz_transformer_; + bool new_color_transformer_; + bool needs_retransform_; + + pluginlib::ClassLoader* transformer_class_loader_; +}; + +} // namespace rtabmap_ros + +#endif diff --git a/rtabmap_demo/src/rviz/MapGraphDisplay.cpp b/rtabmap_demo/src/rviz/MapGraphDisplay.cpp new file mode 100644 index 0000000..f672f2e --- /dev/null +++ b/rtabmap_demo/src/rviz/MapGraphDisplay.cpp @@ -0,0 +1,182 @@ +/* +Copyright (c) 2010-2016, 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 "rviz/properties/color_property.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/int_property.h" + +#include "MapGraphDisplay.h" + +#include +#include + +namespace rtabmap_ros +{ + +MapGraphDisplay::MapGraphDisplay() +{ + color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue, + "Color to draw neighbor links.", this ); + color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0), + "Color to draw merged neighbor links.", this ); + color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red, + "Color to draw global loop closure links.", this ); + color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow, + "Color to draw local loop closure links.", this ); + color_user_property_ = new rviz::ColorProperty( "User", Qt::red, + "Color to draw user links.", this ); + color_virtual_property_ = new rviz::ColorProperty( "Virtual", Qt::magenta, + "Color to draw virtual links.", this ); + + alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0, + "Amount of transparency to apply to the path.", this ); +} + +MapGraphDisplay::~MapGraphDisplay() +{ + destroyObjects(); +} + +void MapGraphDisplay::onInitialize() +{ + MFDClass::onInitialize(); + destroyObjects(); +} + +void MapGraphDisplay::reset() +{ + MFDClass::reset(); + destroyObjects(); +} + +void MapGraphDisplay::destroyObjects() +{ + for(unsigned int i=0; iclear(); + scene_manager_->destroyManualObject( manual_objects_[i] ); + } + manual_objects_.clear(); +} + +void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg ) +{ + if(!(msg->poses.size() == msg->posesId.size())) + { + ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size."); + return; + } + + // Get links + std::map poses; + std::multimap links; + rtabmap::Transform mapToOdom; + rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom); + + destroyObjects(); + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) + { + ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); + } + + Ogre::Matrix4 transform( orientation ); + transform.setTrans( position ); + + if(links.size()) + { + Ogre::ColourValue color; + Ogre::ManualObject* manual_object = scene_manager_->createManualObject(); + manual_object->setDynamic( true ); + scene_node_->attachObject( manual_object ); + manual_objects_.push_back(manual_object); + + manual_object->estimateVertexCount(links.size() * 2); + manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST ); + for(std::map::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + std::map::iterator poseIterFrom = poses.find(iter->second.from()); + std::map::iterator poseIterTo = poses.find(iter->second.to()); + if(poseIterFrom != poses.end() && poseIterTo != poses.end()) + { + if(iter->second.type() == rtabmap::Link::kNeighbor) + { + color = color_neighbor_property_->getOgreColor(); + } + else if(iter->second.type() == rtabmap::Link::kNeighborMerged) + { + color = color_neighbor_merged_property_->getOgreColor(); + } + else if(iter->second.type() == rtabmap::Link::kVirtualClosure) + { + color = color_virtual_property_->getOgreColor(); + } + else if(iter->second.type() == rtabmap::Link::kUserClosure) + { + color = color_user_property_->getOgreColor(); + } + else if(iter->second.type() == rtabmap::Link::kLocalSpaceClosure || iter->second.type() == rtabmap::Link::kLocalTimeClosure) + { + color = color_local_property_->getOgreColor(); + } + else + { + color = color_global_property_->getOgreColor(); + } + color.a = alpha_property_->getFloat(); + Ogre::Vector3 pos; + pos = transform * Ogre::Vector3( poseIterFrom->second.x(), poseIterFrom->second.y(), poseIterFrom->second.z() ); + manual_object->position( pos.x, pos.y, pos.z ); + manual_object->colour( color ); + pos = transform * Ogre::Vector3( poseIterTo->second.x(), poseIterTo->second.y(), poseIterTo->second.z() ); + manual_object->position( pos.x, pos.y, pos.z ); + manual_object->colour( color ); + } + } + + manual_object->end(); + } +} + +} // namespace rtabmap_ros + +#include +PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/MapGraphDisplay.h b/rtabmap_demo/src/rviz/MapGraphDisplay.h new file mode 100644 index 0000000..b17f72f --- /dev/null +++ b/rtabmap_demo/src/rviz/MapGraphDisplay.h @@ -0,0 +1,90 @@ +/* +Copyright (c) 2010-2016, 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 MAP_GRAPH_DISPLAY_H +#define MAP_GRAPH_DISPLAY_H + +#include + +#include + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz +{ +class ColorProperty; +class FloatProperty; +} + +using namespace rviz; + +namespace rtabmap_ros +{ + +/** + * \class MapGraphDisplay + * \brief Displays the graph of rtabmap::MapGraph message + */ +class MapGraphDisplay: public MessageFilterDisplay +{ +Q_OBJECT +public: + MapGraphDisplay(); + virtual ~MapGraphDisplay(); + + /** @brief Overridden from Display. */ + virtual void reset(); + +protected: + /** @brief Overridden from Display. */ + virtual void onInitialize(); + + /** @brief Overridden from MessageFilterDisplay. */ + void processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg ); + +private: + void destroyObjects(); + + std::vector manual_objects_; + + ColorProperty* color_neighbor_property_; + ColorProperty* color_neighbor_merged_property_; + ColorProperty* color_global_property_; + ColorProperty* color_local_property_; + ColorProperty* color_user_property_; + ColorProperty* color_virtual_property_; + FloatProperty* alpha_property_; +}; + +} // namespace rtabmap_ros + +#endif /* MAP_GRAPH_DISPLAY_H */ + diff --git a/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp b/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp new file mode 100644 index 0000000..4df70c0 --- /dev/null +++ b/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp @@ -0,0 +1,76 @@ +/* +Copyright (c) 2010-2016, 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 "OrbitOrientedViewController.h" + +#include +#include +#include +#include +#include +#include + +#include "rviz/properties/float_property.h" +#include "rviz/properties/vector_property.h" +#include "rviz/ogre_helpers/shape.h" + +namespace rtabmap_ros +{ + +void OrbitOrientedViewController::updateCamera() +{ + float distance = distance_property_->getFloat(); + float yaw = yaw_property_->getFloat(); + float pitch = pitch_property_->getFloat(); + + Ogre::Matrix3 rot; + reference_orientation_.ToRotationMatrix(rot); + Ogre::Radian rollTarget, pitchTarget, yawTarget; + rot.ToEulerAnglesXYZ(yawTarget, pitchTarget, rollTarget); + + yaw += rollTarget.valueRadians(); + pitch += pitchTarget.valueRadians(); + + Ogre::Vector3 focal_point = focal_point_property_->getVector(); + + float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x; + float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y; + float z = distance * sin( pitch ) + focal_point.z; + + Ogre::Vector3 pos( x, y, z ); + + camera_->setPosition(pos); + camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z); + camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos)); + + focal_shape_->setPosition( focal_point ); +} + +} + +#include +PLUGINLIB_EXPORT_CLASS( rtabmap_ros::OrbitOrientedViewController, rviz::ViewController ) diff --git a/rtabmap_demo/src/rviz/OrbitOrientedViewController.h b/rtabmap_demo/src/rviz/OrbitOrientedViewController.h new file mode 100644 index 0000000..9cb0594 --- /dev/null +++ b/rtabmap_demo/src/rviz/OrbitOrientedViewController.h @@ -0,0 +1,51 @@ +/* +Copyright (c) 2010-2016, 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 ORBITORIENTEDVIEWCONTROLLER_H_ +#define ORBITORIENTEDVIEWCONTROLLER_H_ + +#include "rviz/default_plugin/view_controllers/orbit_view_controller.h" + +namespace rtabmap_ros +{ + +class OrbitOrientedViewController: public rviz::OrbitViewController +{ +Q_OBJECT +public: + OrbitOrientedViewController() {} + virtual ~OrbitOrientedViewController() {} + +protected: + virtual void updateCamera(); +}; + +} + + + +#endif /* ORBITORIENTEDVIEWCONTROLLER_H_ */ From 9d6d5116db30c9b6c62ab11df75b908c55a3ef1d Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Tue, 27 Mar 2018 14:29:58 +0800 Subject: [PATCH 2/6] fix a bug --- ...\214\346\223\215\344\275\234\346\214\207\345\215\227.md" | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" index 1902bfd..92899ec 100644 --- "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" +++ "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" @@ -5,13 +5,15 @@ $ sudo apt-get install ros-lunar-rtabmap-ros * Kinetic - $ sudo apt-get install ros-kinetic-rtabmap-ros + + $ sudo apt-get install ros-kinetic-rtabmap-ros * Jade $ sudo apt-get install ros-jade-rtabmap-ros * Indigo - $ sudo apt-get install ros-indigo-rtabmap-ros + + $ sudo apt-get install ros-indigo-rtabmap-ros * Hydro From f934f87ccce90fd1418a1768d8c8a0aec7a29562 Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Tue, 27 Mar 2018 14:32:44 +0800 Subject: [PATCH 3/6] fix bug --- ...6\223\215\344\275\234\346\214\207\345\215\227.md" | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" index 92899ec..5d559cd 100644 --- "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" +++ "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" @@ -3,27 +3,27 @@ 1. 根据自己的ROS版本,安装对应的rtabmap-ros * lunar - $ sudo apt-get install ros-lunar-rtabmap-ros + $ sudo apt-get install ros-lunar-rtabmap-ros * Kinetic - $ sudo apt-get install ros-kinetic-rtabmap-ros + $ sudo apt-get install ros-kinetic-rtabmap-ros * Jade - $ sudo apt-get install ros-jade-rtabmap-ros + $ sudo apt-get install ros-jade-rtabmap-ros * Indigo - $ sudo apt-get install ros-indigo-rtabmap-ros + $ sudo apt-get install ros-indigo-rtabmap-ros * Hydro - $ sudo apt-get install ros-hydro-rtabmap-ros + $ sudo apt-get install ros-hydro-rtabmap-ros 2. 从源构建,添加依赖 * 首先检查 `~/.bashrc`文件是否包含下面两句: - $ source /opt/ros/kinetic/setup.bash + $ source /opt/ros/kinetic/setup.bash $ source ~/catkin_ws/devel/setup.bash * 要求安装的依赖: From f604eedc29d43a76833419b2eb3508af82a5bc04 Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Tue, 27 Mar 2018 14:35:17 +0800 Subject: [PATCH 4/6] fix bug --- ...\214\346\223\215\344\275\234\346\214\207\345\215\227.md" | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" index 5d559cd..01d6646 100644 --- "a/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" +++ "b/rtabmap_demo/rtabmap\345\234\250\344\273\277\347\234\237\347\216\257\345\242\203\344\270\213\347\232\204\350\277\220\350\241\214\346\223\215\344\275\234\346\214\207\345\215\227.md" @@ -3,17 +3,17 @@ 1. 根据自己的ROS版本,安装对应的rtabmap-ros * lunar - $ sudo apt-get install ros-lunar-rtabmap-ros + $ sudo apt-get install ros-lunar-rtabmap-ros * Kinetic - $ sudo apt-get install ros-kinetic-rtabmap-ros + $ sudo apt-get install ros-kinetic-rtabmap-ros * Jade $ sudo apt-get install ros-jade-rtabmap-ros * Indigo - $ sudo apt-get install ros-indigo-rtabmap-ros + $ sudo apt-get install ros-indigo-rtabmap-ros * Hydro From fb03a260f6fce41471b1815acecb75fa799a2885 Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Wed, 28 Mar 2018 18:12:23 +0800 Subject: [PATCH 5/6] del Extra bag --- rtabmap_demo/src/nodelets/data_odom_sync.cpp | 130 --- rtabmap_demo/src/nodelets/data_throttle.cpp | 220 ----- .../src/nodelets/disparity_to_depth.cpp | 140 --- rtabmap_demo/src/nodelets/icp_odometry.cpp | 347 ------- .../src/nodelets/obstacles_detection.cpp | 411 -------- .../src/nodelets/obstacles_detection_old.cpp | 357 ------- .../src/nodelets/point_cloud_aggregator.cpp | 112 --- rtabmap_demo/src/nodelets/point_cloud_xyz.cpp | 377 -------- .../src/nodelets/point_cloud_xyzrgb.cpp | 539 ----------- .../src/nodelets/pointcloud_to_depthimage.cpp | 247 ----- rtabmap_demo/src/nodelets/rgbd_odometry.cpp | 654 ------------- rtabmap_demo/src/nodelets/rgbd_sync.cpp | 243 ----- .../src/nodelets/rgbdicp_odometry.cpp | 473 --------- rtabmap_demo/src/nodelets/stereo_odometry.cpp | 347 ------- rtabmap_demo/src/nodelets/stereo_throttle.cpp | 246 ----- rtabmap_demo/src/nodelets/undistort_depth.cpp | 121 --- rtabmap_demo/src/rviz/InfoDisplay.cpp | 131 --- rtabmap_demo/src/rviz/InfoDisplay.h | 70 -- rtabmap_demo/src/rviz/MapCloudDisplay.cpp | 901 ------------------ rtabmap_demo/src/rviz/MapCloudDisplay.h | 196 ---- rtabmap_demo/src/rviz/MapGraphDisplay.cpp | 182 ---- rtabmap_demo/src/rviz/MapGraphDisplay.h | 90 -- .../src/rviz/OrbitOrientedViewController.cpp | 76 -- .../src/rviz/OrbitOrientedViewController.h | 51 - 24 files changed, 6661 deletions(-) delete mode 100644 rtabmap_demo/src/nodelets/data_odom_sync.cpp delete mode 100644 rtabmap_demo/src/nodelets/data_throttle.cpp delete mode 100644 rtabmap_demo/src/nodelets/disparity_to_depth.cpp delete mode 100644 rtabmap_demo/src/nodelets/icp_odometry.cpp delete mode 100644 rtabmap_demo/src/nodelets/obstacles_detection.cpp delete mode 100644 rtabmap_demo/src/nodelets/obstacles_detection_old.cpp delete mode 100644 rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp delete mode 100644 rtabmap_demo/src/nodelets/point_cloud_xyz.cpp delete mode 100644 rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp delete mode 100644 rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp delete mode 100644 rtabmap_demo/src/nodelets/rgbd_odometry.cpp delete mode 100644 rtabmap_demo/src/nodelets/rgbd_sync.cpp delete mode 100644 rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp delete mode 100644 rtabmap_demo/src/nodelets/stereo_odometry.cpp delete mode 100644 rtabmap_demo/src/nodelets/stereo_throttle.cpp delete mode 100644 rtabmap_demo/src/nodelets/undistort_depth.cpp delete mode 100644 rtabmap_demo/src/rviz/InfoDisplay.cpp delete mode 100644 rtabmap_demo/src/rviz/InfoDisplay.h delete mode 100644 rtabmap_demo/src/rviz/MapCloudDisplay.cpp delete mode 100644 rtabmap_demo/src/rviz/MapCloudDisplay.h delete mode 100644 rtabmap_demo/src/rviz/MapGraphDisplay.cpp delete mode 100644 rtabmap_demo/src/rviz/MapGraphDisplay.h delete mode 100644 rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp delete mode 100644 rtabmap_demo/src/rviz/OrbitOrientedViewController.h diff --git a/rtabmap_demo/src/nodelets/data_odom_sync.cpp b/rtabmap_demo/src/nodelets/data_odom_sync.cpp deleted file mode 100644 index 6725af9..0000000 --- a/rtabmap_demo/src/nodelets/data_odom_sync.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "ros/ros.h" -#include "pluginlib/class_list_macros.h" -#include "nodelet/nodelet.h" - -#include -#include -#include - -#include -#include - -#include -#include - -namespace rtabmap_ros -{ - -class DataOdomSyncNodelet : public nodelet::Nodelet -{ -public: - //Constructor - DataOdomSyncNodelet(): - sync_(0) - { - } - - virtual ~DataOdomSyncNodelet() - { - delete sync_; - } - -private: - virtual void onInit() - { - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(private_nh, "rgb"); - ros::NodeHandle depth_pnh(private_nh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - int queueSize = 10; - private_nh.param("queue_size", queueSize, queueSize); - - sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_); - sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, _1, _2, _3, _4)); - - image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info_in", 1); - odom_sub_.subscribe(nh, "odom_in", 1); - - imagePub_ = rgb_it.advertise("image_out", 1); - imageDepthPub_ = depth_it.advertise("image_out", 1); - infoPub_ = rgb_nh.advertise("camera_info_out", 1); - odomPub_ = nh.advertise("odom_out", 1); - }; - - void callback(const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& imageDepth, - const sensor_msgs::CameraInfoConstPtr& camInfo, - const nav_msgs::OdometryConstPtr & odom) - { - if(imagePub_.getNumSubscribers()) - { - imagePub_.publish(image); - } - if(imageDepthPub_.getNumSubscribers()) - { - imageDepthPub_.publish(imageDepth); - } - if(infoPub_.getNumSubscribers()) - { - infoPub_.publish(camInfo); - } - if(odomPub_.getNumSubscribers()) - { - odomPub_.publish(odom); - } - } - - image_transport::Publisher imagePub_; - image_transport::Publisher imageDepthPub_; - ros::Publisher infoPub_; - ros::Publisher odomPub_; - - image_transport::SubscriberFilter image_sub_; - image_transport::SubscriberFilter image_depth_sub_; - message_filters::Subscriber info_sub_; - message_filters::Subscriber odom_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer * sync_; - -}; - - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataOdomSyncNodelet, nodelet::Nodelet); -} diff --git a/rtabmap_demo/src/nodelets/data_throttle.cpp b/rtabmap_demo/src/nodelets/data_throttle.cpp deleted file mode 100644 index 47d266b..0000000 --- a/rtabmap_demo/src/nodelets/data_throttle.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "ros/ros.h" -#include "pluginlib/class_list_macros.h" -#include "nodelet/nodelet.h" - -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include - -namespace rtabmap_ros -{ - -class DataThrottleNodelet : public nodelet::Nodelet -{ -public: - //Constructor - DataThrottleNodelet(): - rate_(0), - approxSync_(0), - exactSync_(0), - decimation_(1) - { - } - - virtual ~DataThrottleNodelet() - { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - } - -private: - ros::Time last_update_; - double rate_; - virtual void onInit() - { - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& private_nh = getPrivateNodeHandle(); - - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(private_nh, "rgb"); - ros::NodeHandle depth_pnh(private_nh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - int queueSize = 10; - bool approxSync = true; - if(private_nh.getParam("max_rate", rate_)) - { - NODELET_WARN("\"max_rate\" is now known as \"rate\"."); - } - private_nh.param("rate", rate_, rate_); - private_nh.param("queue_size", queueSize, queueSize); - private_nh.param("approx_sync", approxSync, approxSync); - private_nh.param("decimation", decimation_, decimation_); - ROS_ASSERT(decimation_ >= 1); - NODELET_INFO("Rate=%f Hz", rate_); - NODELET_INFO("Decimation=%d", decimation_); - NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); - - if(approxSync) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_); - approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3)); - } - else - { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_); - exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3)); - } - - image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info_in", 1); - - imagePub_ = rgb_it.advertise("image_out", 1); - imageDepthPub_ = depth_it.advertise("image_out", 1); - infoPub_ = rgb_nh.advertise("camera_info_out", 1); - }; - - void callback(const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& imageDepth, - const sensor_msgs::CameraInfoConstPtr& camInfo) - { - if (rate_ > 0.0) - { - NODELET_DEBUG("update set to %f", rate_); - if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) - { - NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); - return; - } - } - else - NODELET_DEBUG("rate unset continuing"); - - last_update_ = ros::Time::now(); - - if(imagePub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); - cv_bridge::CvImage out; - out.header = imagePtr->header; - out.encoding = imagePtr->encoding; - out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); - imagePub_.publish(out.toImageMsg()); - } - else - { - imagePub_.publish(image); - } - } - if(imageDepthPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth); - cv_bridge::CvImage out; - out.header = imagePtr->header; - out.encoding = imagePtr->encoding; - out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); - imageDepthPub_.publish(out.toImageMsg()); - } - else - { - imageDepthPub_.publish(imageDepth); - } - } - if(infoPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - sensor_msgs::CameraInfo info = *camInfo; - info.height /= decimation_; - info.width /= decimation_; - info.roi.height /= decimation_; - info.roi.width /= decimation_; - info.K[2]/=float(decimation_); // cx - info.K[5]/=float(decimation_); // cy - info.K[0]/=float(decimation_); // fx - info.K[4]/=float(decimation_); // fy - info.P[2]/=float(decimation_); // cx - info.P[6]/=float(decimation_); // cy - info.P[0]/=float(decimation_); // fx - info.P[5]/=float(decimation_); // fy - infoPub_.publish(info); - } - else - { - infoPub_.publish(camInfo); - } - } - } - - image_transport::Publisher imagePub_; - image_transport::Publisher imageDepthPub_; - ros::Publisher infoPub_; - - image_transport::SubscriberFilter image_sub_; - image_transport::SubscriberFilter image_depth_sub_; - message_filters::Subscriber info_sub_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; - - int decimation_; - -}; - - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataThrottleNodelet, nodelet::Nodelet); -} diff --git a/rtabmap_demo/src/nodelets/disparity_to_depth.cpp b/rtabmap_demo/src/nodelets/disparity_to_depth.cpp deleted file mode 100644 index 3c5628c..0000000 --- a/rtabmap_demo/src/nodelets/disparity_to_depth.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 - -namespace rtabmap_ros -{ - -class DisparityToDepth : public nodelet::Nodelet -{ -public: - DisparityToDepth() {} - - virtual ~DisparityToDepth(){} - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - image_transport::ImageTransport it(nh); - pub32f_ = it.advertise("depth", 1); - pub16u_ = it.advertise("depth_raw", 1); - sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this); - } - - void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg) - { - if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) - { - NODELET_ERROR("Input type must be disparity=32FC1"); - return; - } - - bool publish32f = pub32f_.getNumSubscribers(); - bool publish16u = pub16u_.getNumSubscribers(); - - if(publish32f || publish16u) - { - // sensor_msgs::image_encodings::TYPE_32FC1 - cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); - - cv::Mat depth32f; - cv::Mat depth16u; - if(publish32f) - { - depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F); - } - if(publish16u) - { - depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U); - } - for (int i = 0; i < disparity.rows; i++) - { - for (int j = 0; j < disparity.cols; j++) - { - float disparity_value = disparity.at(i,j); - if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity) - { - // baseline * focal / disparity - float depth = disparityMsg->T * disparityMsg->f / disparity_value; - if(publish32f) - { - depth32f.at(i,j) = depth; - } - if(publish16u) - { - depth16u.at(i,j) = (unsigned short)(depth*1000.0f); - } - } - } - } - - if(publish32f) - { - // convert to ROS sensor_msg::Image - cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f); - sensor_msgs::Image depthMsg; - cvDepth.toImageMsg(depthMsg); - - //publish the message - pub32f_.publish(depthMsg); - } - - if(publish16u) - { - // convert to ROS sensor_msg::Image - cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u); - sensor_msgs::Image depthMsg; - cvDepth.toImageMsg(depthMsg); - - //publish the message - pub16u_.publish(depthMsg); - } - } -} - -private: - image_transport::Publisher pub32f_; - image_transport::Publisher pub16u_; - ros::Subscriber sub_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DisparityToDepth, nodelet::Nodelet); -} diff --git a/rtabmap_demo/src/nodelets/icp_odometry.cpp b/rtabmap_demo/src/nodelets/icp_odometry.cpp deleted file mode 100644 index dda73e0..0000000 --- a/rtabmap_demo/src/nodelets/icp_odometry.cpp +++ /dev/null @@ -1,347 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "rtabmap_ros/MsgConversion.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace rtabmap; - -namespace rtabmap_ros -{ - -class ICPOdometry : public rtabmap_ros::OdometryROS -{ -public: - ICPOdometry() : - OdometryROS(false, false, true), - scanCloudMaxPoints_(0), - scanDownsamplingStep_(1), - scanVoxelSize_(0.0), - scanNormalK_(0), - scanNormalRadius_(0.0) - { - } - - virtual ~ICPOdometry() - { - } - -private: - - virtual void onOdomInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); - pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - pnh.param("scan_normal_k", scanNormalK_, scanNormalK_); - if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k")) - { - ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". " - "The value is still used. Use \"scan_normal_k\" to avoid this warning."); - pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_); - } - pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_); - - NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); - NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_); - NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_); - NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_); - NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_); - - scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this); - cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this); - } - - virtual void updateParameters(ParametersMap & parameters) - { - //make sure we are using Reg/Strategy=0 - ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); - if(iter != parameters.end() && iter->second.compare("1") != 0) - { - ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str()); - } - uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1")); - - ros::NodeHandle & pnh = getPrivateNodeHandle(); - iter = parameters.find(Parameters::kIcpDownsamplingStep()); - if(iter != parameters.end()) - { - int value = uStr2Int(iter->second); - if(value > 1) - { - if(!pnh.hasParam("scan_downsampling_step")) - { - ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str()); - scanDownsamplingStep_ = value; - iter->second = "1"; - } - else - { - ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str()); - } - } - } - iter = parameters.find(Parameters::kIcpVoxelSize()); - if(iter != parameters.end()) - { - float value = uStr2Float(iter->second); - if(value != 0.0f) - { - if(!pnh.hasParam("scan_voxel_size")) - { - ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str()); - scanVoxelSize_ = value; - iter->second = "0"; - } - else - { - ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str()); - } - } - } - iter = parameters.find(Parameters::kIcpPointToPlaneK()); - if(iter != parameters.end()) - { - int value = uStr2Int(iter->second); - if(value != 0) - { - if(!pnh.hasParam("scan_normal_k")) - { - ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str()); - scanNormalK_ = value; - } - } - } - iter = parameters.find(Parameters::kIcpPointToPlaneRadius()); - if(iter != parameters.end()) - { - float value = uStr2Float(iter->second); - if(value != 0.0f) - { - if(!pnh.hasParam("scan_normal_radius")) - { - ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str()); - scanNormalRadius_ = value; - } - } - } - } - - void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) - { - // make sure the frame of the laser is updated too - Transform localScanTransform = getTransform(this->frameId(), - scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); - if(localScanTransform.isNull()) - { - ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); - return; - } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - pclScan->is_dense = true; - - cv::Mat scan; - int maxLaserScans = (int)scanMsg->ranges.size(); - if(pclScan->size()) - { - if(scanDownsamplingStep_ > 1) - { - pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); - maxLaserScans /= scanDownsamplingStep_; - } - if(scanVoxelSize_ > 0.0f) - { - float pointsBeforeFiltering = (float)pclScan->size(); - pclScan = util3d::voxelize(pclScan, scanVoxelSize_); - float ratio = float(pclScan->size()) / pointsBeforeFiltering; - maxLaserScans = int(float(maxLaserScans) * ratio); - } - if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) - { - //compute normals - pcl::PointCloud::Ptr normals; - if(scanVoxelSize_ > 0.0f) - { - normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); - } - else - { - normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_); - } - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScan2dFromPointCloud(*pclScan); - } - } - - rtabmap::SensorData data( - LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform), - cv::Mat(), - cv::Mat(), - CameraModel(), - 0, - rtabmap_ros::timestampFromROS(scanMsg->header.stamp)); - - this->processData(data, scanMsg->header.stamp); - } - - void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg) - { - cv::Mat scan; - bool containNormals = false; - if(scanVoxelSize_ == 0.0f) - { - for(unsigned int i=0; ifields.size(); ++i) - { - if(cloudMsg->fields[i].name.compare("normal_x") == 0) - { - containNormals = true; - break; - } - } - } - - Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); - if(localScanTransform.isNull()) - { - ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); - return; - } - - int maxLaserScans = scanCloudMaxPoints_; - if(containNormals) - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); - if(pclScan->size() && scanDownsamplingStep_ > 1) - { - pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); - maxLaserScans /= scanDownsamplingStep_; - } - scan = util3d::laserScanFromPointCloud(*pclScan); - } - else - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); - if(pclScan->size() && scanDownsamplingStep_ > 1) - { - pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); - maxLaserScans /= scanDownsamplingStep_; - } - if(!pclScan->is_dense) - { - pclScan = util3d::removeNaNFromPointCloud(pclScan); - } - - if(pclScan->size()) - { - if(scanVoxelSize_ > 0.0f) - { - float pointsBeforeFiltering = (float)pclScan->size(); - pclScan = util3d::voxelize(pclScan, scanVoxelSize_); - float ratio = float(pclScan->size()) / pointsBeforeFiltering; - maxLaserScans = int(float(maxLaserScans) * ratio); - } - if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) - { - //compute normals - pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_); - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScanFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScanFromPointCloud(*pclScan); - } - } - } - - rtabmap::SensorData data( - LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform), - cv::Mat(), - cv::Mat(), - CameraModel(), - 0, - rtabmap_ros::timestampFromROS(cloudMsg->header.stamp)); - - this->processData(data, cloudMsg->header.stamp); - } - -protected: - virtual void flushCallbacks() - { - // flush callbacks - } - -private: - ros::Subscriber scan_sub_; - ros::Subscriber cloud_sub_; - int scanCloudMaxPoints_; - int scanDownsamplingStep_; - double scanVoxelSize_; - int scanNormalK_; - double scanNormalRadius_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet); - -} diff --git a/rtabmap_demo/src/nodelets/obstacles_detection.cpp b/rtabmap_demo/src/nodelets/obstacles_detection.cpp deleted file mode 100644 index 81c3340..0000000 --- a/rtabmap_demo/src/nodelets/obstacles_detection.cpp +++ /dev/null @@ -1,411 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "rtabmap/core/OccupancyGrid.h" -#include "rtabmap/utilite/UStl.h" - -namespace rtabmap_ros -{ - -class ObstaclesDetection : public nodelet::Nodelet -{ -public: - ObstaclesDetection() : - frameId_("base_link"), - waitForTransform_(false), - mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()) - {} - - virtual ~ObstaclesDetection() - {} - -private: - - void parameterMoved( - ros::NodeHandle & nh, - const std::string & rosName, - const std::string & parameterName, - rtabmap::ParametersMap & parameters) - { - if(nh.hasParam(rosName)) - { - rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); - rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName); - if(iter != gridParameters.end()) - { - NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from " - "rtabmap_ros to rtabmap library. Use " - "parameter \"%s\" instead. The value is still " - "copied to new parameter name.", - rosName.c_str(), - parameterName.c_str()); - std::string type = rtabmap::Parameters::getType(parameterName); - if(type.compare("float") || type.compare("double")) - { - double v = uStr2Double(iter->second); - nh.getParam(rosName, v); - parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); - } - else if(type.compare("int") || type.compare("unsigned int")) - { - int v = uStr2Int(iter->second); - nh.getParam(rosName, v); - parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v))); - } - else - { - NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); - } - } - else - { - NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); - } - } - } - - virtual void onInit() - { - ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!? - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); - pnh.param("frame_id", frameId_, frameId_); - pnh.param("map_frame_id", mapFrameId_, mapFrameId_); - pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); - - if(pnh.hasParam("optimize_for_close_objects")) - { - NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist " - "anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use " - "the old interface."); - } - - rtabmap::ParametersMap parameters; - - // Backward compatibility - for(std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin(); - iter!=rtabmap::Parameters::getRemovedParameters().end(); - ++iter) - { - std::string vStr; - if(pnh.getParam(iter->first, vStr)) - { - if(iter->second.first) - { - // can be migrated - uInsert(parameters, rtabmap::ParametersPair(iter->second.second, vStr)); - NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", - iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); - } - else - { - if(iter->second.second.empty()) - { - NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!", - iter->first.c_str()); - } - else - { - NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", - iter->first.c_str(), iter->second.second.c_str()); - } - } - } - } - - rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters(); - rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid"); - for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter) - { - std::string vStr; - bool vBool; - int vInt; - double vDouble; - if(pnh.getParam(iter->first, vStr)) - { - NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); - iter->second = vStr; - } - else if(pnh.getParam(iter->first, vBool)) - { - NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); - } - else if(pnh.getParam(iter->first, vDouble)) - { - NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); - } - else if(pnh.getParam(iter->first, vInt)) - { - NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); - } - } - uInsert(parameters, gridParameters); - parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters); - parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters); - parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters); - parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); - parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters); - parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters); - parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters); - parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters); - parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters); - - UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection())); - mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())); - if(mapFrameProjection_ && mapFrameId_.empty()) - { - NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); - } - - grid_.parseParameters(parameters); - - cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); - - groundPub_ = nh.advertise("ground", 1); - obstaclesPub_ = nh.advertise("obstacles", 1); - projObstaclesPub_ = nh.advertise("proj_obstacles", 1); - } - - - - void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - ros::WallTime time = ros::WallTime::now(); - - if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) - { - // no one wants the results - return; - } - - rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); - try - { - if(waitForTransform_) - { - if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) - { - NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); - return; - } - } - tf::StampedTransform tmp; - tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); - localTransform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - NODELET_ERROR("%s",ex.what()); - return; - } - - rtabmap::Transform pose = rtabmap::Transform::getIdentity(); - if(!mapFrameId_.empty()) - { - try - { - if(waitForTransform_) - { - if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1))) - { - NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str()); - return; - } - } - tf::StampedTransform tmp; - tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp); - pose = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - NODELET_ERROR("%s",ex.what()); - return; - } - } - - pcl::PointCloud::Ptr inputCloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *inputCloud); - - //Common variables for all strategies - pcl::IndicesPtr ground, obstacles; - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); - - if(inputCloud->size()) - { - inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); - - pcl::IndicesPtr flatObstacles(new std::vector); - pcl::PointCloud::Ptr cloud = grid_.segmentCloud( - inputCloud, - pcl::IndicesPtr(new std::vector), - pose, - cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()), - ground, - obstacles, - &flatObstacles); - - if(cloud->size() && (ground->size() || obstacles->size())) - { - if(groundPub_.getNumSubscribers() && - ground.get() && ground->size()) - { - pcl::copyPointCloud(*cloud, *ground, *groundCloud); - } - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && - obstacles.get() && obstacles->size()) - { - // remove flat obstacles from obstacles - std::set flatObstaclesSet; - if(projObstaclesPub_.getNumSubscribers()) - { - flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); - } - - obstaclesCloud->resize(obstacles->size()); - obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); - - int oi=0; - for(unsigned int i=0; isize(); ++i) - { - obstaclesCloud->points[i] = cloud->at(obstacles->at(i)); - if(flatObstaclesSet.size() == 0 || - flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) - { - obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; - obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; - ++oi; - } - - } - - obstaclesCloudWithoutFlatSurfaces->resize(oi); - } - - if(!localTransform.isIdentity() || !pose.isIdentity()) - { - //transform back in topic frame for 3d clouds and base frame for 2d clouds - - float roll, pitch, yaw; - pose.getEulerAngles(roll, pitch, yaw); - rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0); - - if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity()) - { - obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse()); - } - - t = (t*localTransform).inverse(); - if(groundCloud->size()) - { - groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t); - } - if(obstaclesCloud->size()) - { - obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t); - } - } - } - } - - if(groundPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header = cloudMsg->header; - - //publish the message - groundPub_.publish(rosCloud); - } - - if(obstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header = cloudMsg->header; - - //publish the message - obstaclesPub_.publish(rosCloud); - } - - if(projObstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - projObstaclesPub_.publish(rosCloud); - } - - NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); - } - -private: - std::string frameId_; - std::string mapFrameId_; - bool waitForTransform_; - - rtabmap::OccupancyGrid grid_; - bool mapFrameProjection_; - - tf::TransformListener tfListener_; - - ros::Publisher groundPub_; - ros::Publisher obstaclesPub_; - ros::Publisher projObstaclesPub_; - - ros::Subscriber cloudSub_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet); -} - - diff --git a/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp b/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp deleted file mode 100644 index 40bcd47..0000000 --- a/rtabmap_demo/src/nodelets/obstacles_detection_old.cpp +++ /dev/null @@ -1,357 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_mapping.h" -#include "rtabmap/core/util3d_transforms.h" - -namespace rtabmap_ros -{ - -class ObstaclesDetectionOld : public nodelet::Nodelet -{ -public: - ObstaclesDetectionOld() : - frameId_("base_link"), - normalKSearch_(20), - groundNormalAngle_(M_PI_4), - clusterRadius_(0.05), - minClusterSize_(20), - maxObstaclesHeight_(0.0), // if<=0.0 -> disabled - maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true - segmentFlatObstacles_(false), - waitForTransform_(false), - optimizeForCloseObjects_(false), - projVoxelSize_(0.01) - {} - - virtual ~ObstaclesDetectionOld() - {} - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); - pnh.param("frame_id", frameId_, frameId_); - pnh.param("normal_k", normalKSearch_, normalKSearch_); - pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); - if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius")) - { - NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed " - "to \"cluster_radius\"! Your value is still copied to " - "corresponding parameter. Instead of normal radius, nearest neighbors count " - "\"normal_k\" is used instead (default 20)."); - pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_); - } - else - { - pnh.param("cluster_radius", clusterRadius_, clusterRadius_); - } - pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); - pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); - pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_); - pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_); - pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); - pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); - pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_); - - cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this); - - groundPub_ = nh.advertise("ground", 1); - obstaclesPub_ = nh.advertise("obstacles", 1); - projObstaclesPub_ = nh.advertise("proj_obstacles", 1); - } - - - - void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) - { - ros::WallTime time = ros::WallTime::now(); - - if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0) - { - // no one wants the results - return; - } - - rtabmap::Transform localTransform; - try - { - if(waitForTransform_) - { - if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) - { - NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); - return; - } - } - tf::StampedTransform tmp; - tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); - localTransform = rtabmap_ros::transformFromTF(tmp); - } - catch(tf::TransformException & ex) - { - NODELET_ERROR("%s",ex.what()); - return; - } - - pcl::PointCloud::Ptr originalCloud(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *originalCloud); - - //Common variables for all strategies - pcl::IndicesPtr ground, obstacles; - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud); - - if(originalCloud->size()) - { - originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); - if(maxObstaclesHeight_ > 0) - { - // std::numeric_limits::lowest() exists only for c++11 - originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits::min(), maxObstaclesHeight_); - } - - if(originalCloud->size()) - { - if(!optimizeForCloseObjects_) - { - // This is the default strategy - pcl::IndicesPtr flatObstacles(new std::vector); - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud, - ground, - obstacles, - normalKSearch_, - groundNormalAngle_, - clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_, - &flatObstacles); - - if(groundPub_.getNumSubscribers() && - ground.get() && ground->size()) - { - pcl::copyPointCloud(*originalCloud, *ground, *groundCloud); - } - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && - obstacles.get() && obstacles->size()) - { - // remove flat obstacles from obstacles - std::set flatObstaclesSet; - if(projObstaclesPub_.getNumSubscribers()) - { - flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end()); - } - - obstaclesCloud->resize(obstacles->size()); - obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size()); - - int oi=0; - for(unsigned int i=0; isize(); ++i) - { - obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i)); - if(flatObstaclesSet.size() == 0 || - flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) - { - obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; - obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0; - ++oi; - } - - } - - obstaclesCloudWithoutFlatSurfaces->resize(oi); - if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0) - { - obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_); - } - } - } - else - { - // in this case optimizeForCloseObject_ is true: - // we divide the floor point cloud into two subsections, one for all potential floor points up to 1m - // one for potential floor points further away than 1m. - // For the points at closer range, we use a smaller normal estimation radius and ground normal angle, - // which allows to detect smaller objects, without increasing the number of false positive. - // For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the - // grond normal angle (* 2.). - - pcl::PointCloud::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits::min(), 1.); - pcl::PointCloud::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits::max()); - - // Part 1: segment floor and obstacles near the robot - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud_near, - ground, - obstacles, - normalKSearch_, - groundNormalAngle_, - clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_); - - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) - { - pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud); - ground->clear(); - } - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) - { - pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud); - obstacles->clear(); - } - - // Part 2: segment floor and obstacles far from the robot - rtabmap::util3d::segmentObstaclesFromGround( - originalCloud_far, - ground, - obstacles, - normalKSearch_, - 2.*groundNormalAngle_, - 3.*clusterRadius_, - minClusterSize_, - segmentFlatObstacles_, - maxGroundHeight_); - - if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) - { - pcl::PointCloud::Ptr groundCloud2 (new pcl::PointCloud); - pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2); - *groundCloud += *groundCloud2; - } - - - if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size()) - { - pcl::PointCloud::Ptr obstacles2(new pcl::PointCloud); - pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2); - *obstaclesCloud += *obstacles2; - } - } - - if(!localTransform.isIdentity()) - { - //transform back in topic frame - rtabmap::Transform localTransformInv = localTransform.inverse(); - if(groundCloud->size()) - { - groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv); - } - if(obstaclesCloud->size()) - { - obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv); - } - } - } - } - - if(groundPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*groundCloud, rosCloud); - rosCloud.header = cloudMsg->header; - - //publish the message - groundPub_.publish(rosCloud); - } - - if(obstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloud, rosCloud); - rosCloud.header = cloudMsg->header; - - //publish the message - obstaclesPub_.publish(rosCloud); - } - - if(projObstaclesPub_.getNumSubscribers()) - { - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud); - rosCloud.header.stamp = cloudMsg->header.stamp; - rosCloud.header.frame_id = frameId_; - - //publish the message - projObstaclesPub_.publish(rosCloud); - } - - NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); - } - -private: - std::string frameId_; - int normalKSearch_; - double groundNormalAngle_; - double clusterRadius_; - int minClusterSize_; - double maxObstaclesHeight_; - double maxGroundHeight_; - bool segmentFlatObstacles_; - bool waitForTransform_; - bool optimizeForCloseObjects_; - double projVoxelSize_; - - tf::TransformListener tfListener_; - - ros::Publisher groundPub_; - ros::Publisher obstaclesPub_; - ros::Publisher projObstaclesPub_; - - ros::Subscriber cloudSub_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetectionOld, nodelet::Nodelet); -} - - diff --git a/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp deleted file mode 100644 index bcdc776..0000000 --- a/rtabmap_demo/src/nodelets/point_cloud_aggregator.cpp +++ /dev/null @@ -1,112 +0,0 @@ - -#include -#include -#include - -#include -#include -#include - -#include - -#include - -#include - -#include -#include - -#include -#include -#include - -#include - -namespace rtabmap_ros -{ - -class PointCloudAggregator : public nodelet::Nodelet -{ -public: - PointCloudAggregator() : sync_(NULL) - {} - - virtual ~PointCloudAggregator() - { - if (sync_!=NULL) delete sync_; - } - -private: - void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1, - const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2, - const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3) - { - if(cloudPub_.getNumSubscribers()) - { - pcl::PointCloud cloud1, cloud2, cloud3; - - pcl::fromROSMsg(*cloudMsg_1, cloud1); - std::string frameId = frameId_; - if(!frameId.empty() && frameId.compare(cloudMsg_1->header.frame_id) != 0) - { - pcl_ros::transformPointCloud(frameId, cloud1, cloud1, tfListener_); - } - else - { - frameId = cloudMsg_1->header.frame_id; - } - pcl::fromROSMsg(*cloudMsg_2, cloud2); - if(frameId.compare(cloudMsg_2->header.frame_id) != 0) - { - pcl_ros::transformPointCloud(frameId, cloud2, cloud2, tfListener_); - } - pcl::fromROSMsg(*cloudMsg_3, cloud3); - if(frameId.compare(cloudMsg_3->header.frame_id) != 0) - { - pcl_ros::transformPointCloud(frameId, cloud3, cloud3, tfListener_); - } - pcl::PointCloud totalCloud; - totalCloud = cloud1 + cloud2; - totalCloud += cloud3; - sensor_msgs::PointCloud2 rosCloud; - pcl::toROSMsg(totalCloud, rosCloud); - rosCloud.header.stamp = cloudMsg_1->header.stamp; - rosCloud.header.frame_id = frameId; - cloudPub_.publish(rosCloud); - } - } - - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 5; - pnh.param("queue_size", queueSize, queueSize); - pnh.param("frame_id", frameId_, frameId_); - - cloudSub_1_.subscribe(nh, "cloud1", 1); - cloudSub_2_.subscribe(nh, "cloud2", 1); - cloudSub_3_.subscribe(nh, "cloud3", 1); - - sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); - sync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3)); - - cloudPub_ = nh.advertise("combined_cloud", 1); - } - - message_filters::Synchronizer* sync_; - message_filters::Subscriber cloudSub_1_; - message_filters::Subscriber cloudSub_2_; - message_filters::Subscriber cloudSub_3_; - - ros::Publisher cloudPub_; - - std::string frameId_; - tf::TransformListener tfListener_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet); -} - diff --git a/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp b/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp deleted file mode 100644 index 0b322d3..0000000 --- a/rtabmap_demo/src/nodelets/point_cloud_xyz.cpp +++ /dev/null @@ -1,377 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#include -#include - -#include "rtabmap/core/util2d.h" -#include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_surface.h" -#include "rtabmap/utilite/UConversion.h" -#include "rtabmap/utilite/UStl.h" - -namespace rtabmap_ros -{ - -class PointCloudXYZ : public nodelet::Nodelet -{ -public: - PointCloudXYZ() : - maxDepth_(0.0), - minDepth_(0.0), - voxelSize_(0.0), - decimation_(1), - noiseFilterRadius_(0.0), - noiseFilterMinNeighbors_(5), - normalK_(0), - normalRadius_(0.0), - filterNaNs_(false), - approxSyncDepth_(0), - approxSyncDisparity_(0), - exactSyncDepth_(0), - exactSyncDisparity_(0) - {} - - virtual ~PointCloudXYZ() - { - if(approxSyncDepth_) - delete approxSyncDepth_; - if(approxSyncDisparity_) - delete approxSyncDisparity_; - if(exactSyncDepth_) - delete exactSyncDepth_; - if(exactSyncDisparity_) - delete exactSyncDisparity_; - } - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - bool approxSync = true; - std::string roiStr; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize, queueSize); - pnh.param("max_depth", maxDepth_, maxDepth_); - pnh.param("min_depth", minDepth_, minDepth_); - pnh.param("voxel_size", voxelSize_, voxelSize_); - pnh.param("decimation", decimation_, decimation_); - pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); - pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - pnh.param("normal_k", normalK_, normalK_); - pnh.param("normal_radius", normalRadius_, normalRadius_); - pnh.param("filter_nans", filterNaNs_, filterNaNs_); - pnh.param("roi_ratios", roiStr, roiStr); - - // Deprecated - if(pnh.hasParam("cut_left")) - { - ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored."); - } - if(pnh.hasParam("cut_right")) - { - ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored."); - } - if(pnh.hasParam("special_filter_close_object")) - { - ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing " - "should be done before or after this nodelet. See old implementation here: " - "https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201."); - } - - //parse roi (region of interest) - roiRatios_.resize(4, 0); - if(!roiStr.empty()) - { - std::list strValues = uSplit(roiStr, ' '); - if(strValues.size() != 4) - { - ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str()); - } - else - { - std::vector tmpValues(4); - unsigned int i=0; - for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) - { - tmpValues[i] = uStr2Float(*jter); - ++i; - } - - if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && - tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && - tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && - tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) - { - roiRatios_ = tmpValues; - } - else - { - ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str()); - } - } - } - - NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); - - if(approxSync) - { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); - approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); - - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); - approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); - } - else - { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); - exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); - - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); - exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); - } - - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); - - disparitySub_.subscribe(nh, "disparity/image", 1); - disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); - - cloudPub_ = nh.advertise("cloud", 1); - } - - void callback( - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) - { - NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); - return; - } - - if(cloudPub_.getNumSubscribers()) - { - ros::WallTime time = ros::WallTime::now(); - - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); - cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); - - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - - pcl::PointCloud::Ptr pclCloud; - rtabmap::CameraModel m( - model.fx(), - model.fy(), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); - - pcl::IndicesPtr indices(new std::vector); - pclCloud = rtabmap::util3d::cloudFromDepth( - cv::Mat(imageDepthPtr->image, roi), - m, - decimation_, - maxDepth_, - minDepth_, - indices.get()); - processAndPublish(pclCloud, indices, depth->header); - - NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); - } - } - - void callbackDisparity( - const stereo_msgs::DisparityImageConstPtr& disparityMsg, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && - disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) - { - NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); - return; - } - - cv::Mat disparity; - if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0) - { - disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); - } - else - { - disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast(disparityMsg->image.data.data())); - } - - if(cloudPub_.getNumSubscribers()) - { - ros::WallTime time = ros::WallTime::now(); - - cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_); - - pcl::PointCloud::Ptr pclCloud; - rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); - rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T); - pcl::IndicesPtr indices(new std::vector); - pclCloud = rtabmap::util3d::cloudFromDisparity( - cv::Mat(disparity, roi), - stereoModel, - decimation_, - maxDepth_, - minDepth_, - indices.get()); - - processAndPublish(pclCloud, indices, disparityMsg->header); - - NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec()); - } - } - - void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) - { - if(indices->size() && voxelSize_ > 0.0) - { - pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); - } - - // Do radius filtering after voxel filtering ( a lot faster) - if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) - { - if(pclCloud->is_dense) - { - indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); - } - else - { - indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); - } - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*pclCloud, *indices, *tmp); - pclCloud = tmp; - } - - sensor_msgs::PointCloud2 rosCloud; - if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) - { - //compute normals - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); - pcl::PointCloud::Ptr pclCloudNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal); - if(filterNaNs_) - { - pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); - } - pcl::toROSMsg(*pclCloudNormal, rosCloud); - } - else - { - if(filterNaNs_ && !pclCloud->is_dense) - { - pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); - } - pcl::toROSMsg(*pclCloud, rosCloud); - } - rosCloud.header.stamp = header.stamp; - rosCloud.header.frame_id = header.frame_id; - - //publish the message - cloudPub_.publish(rosCloud); - } - -private: - - double maxDepth_; - double minDepth_; - double voxelSize_; - int decimation_; - double noiseFilterRadius_; - int noiseFilterMinNeighbors_; - int normalK_; - double normalRadius_; - bool filterNaNs_; - std::vector roiRatios_; - - ros::Publisher cloudPub_; - - image_transport::SubscriberFilter imageDepthSub_; - message_filters::Subscriber cameraInfoSub_; - - message_filters::Subscriber disparitySub_; - message_filters::Subscriber disparityCameraInfoSub_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; - message_filters::Synchronizer * approxSyncDepth_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDisparityPolicy; - message_filters::Synchronizer * approxSyncDisparity_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; - message_filters::Synchronizer * exactSyncDepth_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncDisparityPolicy; - message_filters::Synchronizer * exactSyncDisparity_; - -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet); -} - diff --git a/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp deleted file mode 100644 index 091d1a2..0000000 --- a/rtabmap_demo/src/nodelets/point_cloud_xyzrgb.cpp +++ /dev/null @@ -1,539 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include - -#include - -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - -#include "rtabmap/core/util2d.h" -#include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_surface.h" -#include "rtabmap/utilite/UConversion.h" -#include "rtabmap/utilite/UStl.h" - -namespace rtabmap_ros -{ - -class PointCloudXYZRGB : public nodelet::Nodelet -{ -public: - PointCloudXYZRGB() : - maxDepth_(0.0), - minDepth_(0.0), - voxelSize_(0.0), - decimation_(1), - noiseFilterRadius_(0.0), - noiseFilterMinNeighbors_(5), - normalK_(0), - normalRadius_(0.0), - filterNaNs_(false), - approxSyncDepth_(0), - approxSyncDisparity_(0), - approxSyncStereo_(0), - exactSyncDepth_(0), - exactSyncDisparity_(0), - exactSyncStereo_(0) - {} - - virtual ~PointCloudXYZRGB() - { - if(approxSyncDepth_) - delete approxSyncDepth_; - if(approxSyncDisparity_) - delete approxSyncDisparity_; - if(approxSyncStereo_) - delete approxSyncStereo_; - if(exactSyncDepth_) - delete exactSyncDepth_; - if(exactSyncDisparity_) - delete exactSyncDisparity_; - if(exactSyncStereo_) - delete exactSyncStereo_; - } - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - bool approxSync = true; - std::string roiStr; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize, queueSize); - pnh.param("max_depth", maxDepth_, maxDepth_); - pnh.param("min_depth", minDepth_, minDepth_); - pnh.param("voxel_size", voxelSize_, voxelSize_); - pnh.param("decimation", decimation_, decimation_); - pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); - pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - pnh.param("normal_k", normalK_, normalK_); - pnh.param("normal_radius", normalRadius_, normalRadius_); - pnh.param("filter_nans", filterNaNs_, filterNaNs_); - pnh.param("roi_ratios", roiStr, roiStr); - - //parse roi (region of interest) - roiRatios_.resize(4, 0); - if(!roiStr.empty()) - { - std::list strValues = uSplit(roiStr, ' '); - if(strValues.size() != 4) - { - ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str()); - } - else - { - std::vector tmpValues(4); - unsigned int i=0; - for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) - { - tmpValues[i] = uStr2Float(*jter); - ++i; - } - - if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && - tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && - tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && - tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) - { - roiRatios_ = tmpValues; - } - else - { - ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str()); - } - } - } - - // StereoBM parameters - stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM"); - for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter) - { - std::string vStr; - bool vBool; - int vInt; - double vDouble; - if(pnh.getParam(iter->first, vStr)) - { - NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); - iter->second = vStr; - } - else if(pnh.getParam(iter->first, vBool)) - { - NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); - } - else if(pnh.getParam(iter->first, vDouble)) - { - NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); - } - else if(pnh.getParam(iter->first, vInt)) - { - NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); - } - } - - NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); - - cloudPub_ = nh.advertise("cloud", 1); - - if(approxSync) - { - - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); - approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3)); - - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); - approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3)); - - approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); - approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4)); - } - else - { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); - exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3)); - - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); - exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3)); - - exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); - exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4)); - } - - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); - - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - imageDisparitySub_.subscribe(nh, "disparity", 1); - - imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); - imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - } - - void depthCallback( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& imageDepth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || - !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || - imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || - imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); - return; - } - - if(cloudPub_.getNumSubscribers()) - { - ros::WallTime time = ros::WallTime::now(); - - cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); - } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); - } - - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); - - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - - ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols); - ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows); - - pcl::PointCloud::Ptr pclCloud; - cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); - - rtabmap::CameraModel m( - model.fx(), - model.fy(), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); - pcl::IndicesPtr indices(new std::vector); - pclCloud = rtabmap::util3d::cloudFromDepthRGB( - cv::Mat(imagePtr->image, roi), - cv::Mat(imageDepthPtr->image, roi), - m, - decimation_, - maxDepth_, - minDepth_, - indices.get()); - - - processAndPublish(pclCloud, indices, imagePtr->header); - - NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec()); - } - } - - void disparityCallback( - const sensor_msgs::ImageConstPtr& image, - const stereo_msgs::DisparityImageConstPtr& imageDisparity, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); - } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); - } - - if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && - imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) - { - NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); - return; - } - - cv::Mat disparity; - if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0) - { - disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast(imageDisparity->image.data.data())); - } - else - { - disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast(imageDisparity->image.data.data())); - } - - if(cloudPub_.getNumSubscribers()) - { - ros::WallTime time = ros::WallTime::now(); - - cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_); - - pcl::PointCloud::Ptr pclCloud; - rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); - rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T); - pcl::IndicesPtr indices(new std::vector); - pclCloud = rtabmap::util3d::cloudFromDisparityRGB( - cv::Mat(imagePtr->image, roi), - cv::Mat(disparity, roi), - stereoModel, - decimation_, - maxDepth_, - minDepth_, - indices.get()); - - processAndPublish(pclCloud, indices, imageDisparity->header); - - NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec()); - } - } - - void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft, - const sensor_msgs::ImageConstPtr& imageRight, - const sensor_msgs::CameraInfoConstPtr& camInfoLeft, - const sensor_msgs::CameraInfoConstPtr& camInfoRight) - { - if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || - imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); - return; - } - - if(cloudPub_.getNumSubscribers()) - { - ros::WallTime time = ros::WallTime::now(); - - cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; - if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); - } - else - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); - } - ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); - - if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f) - { - ROS_WARN("\"roi_ratios\" set but ignored for stereo images."); - } - - pcl::PointCloud::Ptr pclCloud; - pcl::IndicesPtr indices(new std::vector); - pclCloud = rtabmap::util3d::cloudFromStereoImages( - ptrLeftImage->image, - ptrRightImage->image, - rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), - decimation_, - maxDepth_, - minDepth_, - indices.get(), - stereoBMParameters_); - - processAndPublish(pclCloud, indices, imageLeft->header); - - NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec()); - } - } - - void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) - { - if(indices->size() && voxelSize_ > 0.0) - { - pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); - } - - // Do radius filtering after voxel filtering ( a lot faster) - if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) - { - if(pclCloud->is_dense) - { - indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); - } - else - { - indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); - } - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*pclCloud, *indices, *tmp); - pclCloud = tmp; - } - - sensor_msgs::PointCloud2 rosCloud; - if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f)) - { - //compute normals - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_); - pcl::PointCloud::Ptr pclCloudNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal); - if(filterNaNs_) - { - pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal); - } - pcl::toROSMsg(*pclCloudNormal, rosCloud); - } - else - { - if(filterNaNs_ && !pclCloud->is_dense) - { - pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); - } - pcl::toROSMsg(*pclCloud, rosCloud); - } - rosCloud.header.stamp = header.stamp; - rosCloud.header.frame_id = header.frame_id; - - //publish the message - cloudPub_.publish(rosCloud); - } - -private: - - double maxDepth_; - double minDepth_; - double voxelSize_; - int decimation_; - double noiseFilterRadius_; - int noiseFilterMinNeighbors_; - int normalK_; - double normalRadius_; - bool filterNaNs_; - std::vector roiRatios_; - rtabmap::ParametersMap stereoBMParameters_; - - ros::Publisher cloudPub_; - - image_transport::SubscriberFilter imageSub_; - image_transport::SubscriberFilter imageDepthSub_; - message_filters::Subscriber cameraInfoSub_; - - message_filters::Subscriber imageDisparitySub_; - - image_transport::SubscriberFilter imageLeft_; - image_transport::SubscriberFilter imageRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; - message_filters::Synchronizer * approxSyncDepth_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDisparityPolicy; - message_filters::Synchronizer * approxSyncDisparity_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncStereoPolicy; - message_filters::Synchronizer * approxSyncStereo_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; - message_filters::Synchronizer * exactSyncDepth_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncDisparityPolicy; - message_filters::Synchronizer * exactSyncDisparity_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncStereoPolicy; - message_filters::Synchronizer * exactSyncStereo_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet); -} - diff --git a/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp deleted file mode 100644 index d4db77a..0000000 --- a/rtabmap_demo/src/nodelets/pointcloud_to_depthimage.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include - -namespace rtabmap_ros -{ - -class PointCloudToDepthImage : public nodelet::Nodelet -{ -public: - PointCloudToDepthImage() : - listener_(0), - waitForTransform_(0.1), - fillHolesSize_ (0), - fillHolesError_(0.1), - fillIterations_(1), - decimation_(1), - approxSync_(0), - exactSync_(0) - {} - - virtual ~PointCloudToDepthImage() - { - delete listener_; - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - } - -private: - virtual void onInit() - { - listener_ = new tf::TransformListener(); - - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - bool approx = true; - pnh.param("queue_size", queueSize, queueSize); - pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); - pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); - pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_); - pnh.param("fill_holes_error", fillHolesError_, fillHolesError_); - pnh.param("fill_iterations", fillIterations_, fillIterations_); - pnh.param("decimation", decimation_, decimation_); - pnh.param("approx", approx, approx); - - if(fixedFrameId_.empty() && approx) - { - ROS_FATAL("fixed_frame_id should be set when using approximate " - "time synchronization (approx=true)! If the robot " - "is moving, it could be \"odom\". If not moving, it " - "could be \"base_link\"."); - } - - ROS_INFO("Params:"); - ROS_INFO(" approx=%s", approx?"true":"false"); - ROS_INFO(" queue_size=%d", queueSize); - ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str()); - ROS_INFO(" wait_for_transform=%fs", waitForTransform_); - ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); - ROS_INFO(" fill_holes_error=%f", fillHolesError_); - ROS_INFO(" fill_iterations=%d", fillIterations_); - ROS_INFO(" decimation=%d", decimation_); - - image_transport::ImageTransport it(nh); - depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm - depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters - - if(approx) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); - approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2)); - } - else - { - fixedFrameId_.clear(); - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); - exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2)); - } - - pointCloudSub_.subscribe(nh, "cloud", 1); - cameraInfoSub_.subscribe(nh, "camera_info", 1); - } - - void callback( - const sensor_msgs::PointCloud2ConstPtr & pointCloud2Msg, - const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg) - { - if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0) - { - rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity(); - if(!fixedFrameId_.empty()) - { - // approx sync - cloudDisplacement = rtabmap_ros::getTransform( - pointCloud2Msg->header.frame_id, - fixedFrameId_, - pointCloud2Msg->header.stamp, - cameraInfoMsg->header.stamp, - *listener_, - waitForTransform_); - } - - if(cloudDisplacement.isNull()) - { - return; - } - - rtabmap::Transform cloudToCamera = rtabmap_ros::getTransform( - pointCloud2Msg->header.frame_id, - cameraInfoMsg->header.frame_id, - cameraInfoMsg->header.stamp, - *listener_, - waitForTransform_); - - if(cloudToCamera.isNull()) - { - return; - } - - rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera; - - rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform); - - if(decimation_ > 1) - { - if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0) - { - model = model.scaled(1.0f/float(decimation_)); - } - else - { - ROS_ERROR("decimation (%d) not valid for image size %dx%d", - decimation_, - model.imageWidth(), - model.imageHeight()); - } - } - - pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*pointCloud2Msg, *cloud); - - cv_bridge::CvImage depthImage; - depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform()); - - if(fillHolesSize_ > 0 && fillIterations_ > 0) - { - for(int i=0; iheader; - - if(depthImage32Pub_.getNumSubscribers()) - { - depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - depthImage32Pub_.publish(depthImage.toImageMsg()); - } - - if(depthImage16Pub_.getNumSubscribers()) - { - depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image); - depthImage16Pub_.publish(depthImage.toImageMsg()); - } - } - } - -private: - image_transport::Publisher depthImage16Pub_; - image_transport::Publisher depthImage32Pub_; - message_filters::Subscriber pointCloudSub_; - message_filters::Subscriber cameraInfoSub_; - std::string fixedFrameId_; - tf::TransformListener * listener_; - double waitForTransform_; - int fillHolesSize_; - double fillHolesError_; - int fillIterations_; - int decimation_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudToDepthImage, nodelet::Nodelet); -} diff --git a/rtabmap_demo/src/nodelets/rgbd_odometry.cpp b/rtabmap_demo/src/nodelets/rgbd_odometry.cpp deleted file mode 100644 index f71ce5b..0000000 --- a/rtabmap_demo/src/nodelets/rgbd_odometry.cpp +++ /dev/null @@ -1,654 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include -#include - -#include "rtabmap_ros/MsgConversion.h" - -#include -#include -#include -#include -#include - -using namespace rtabmap; - -namespace rtabmap_ros -{ - -class RGBDOdometry : public rtabmap_ros::OdometryROS -{ -public: - RGBDOdometry() : - OdometryROS(false, true, false), - approxSync_(0), - exactSync_(0), - approxSync2_(0), - exactSync2_(0), - approxSync3_(0), - exactSync3_(0), - approxSync4_(0), - exactSync4_(0), - queueSize_(5) - { - } - - virtual ~RGBDOdometry() - { - rgbdSub_.shutdown(); - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - if(approxSync2_) - { - delete approxSync2_; - } - if(exactSync2_) - { - delete exactSync2_; - } - if(approxSync3_) - { - delete approxSync3_; - } - if(exactSync3_) - { - delete exactSync3_; - } - if(approxSync4_) - { - delete approxSync4_; - } - if(exactSync4_) - { - delete exactSync4_; - } - } - -private: - - virtual void onOdomInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int rgbdCameras = 1; - bool approxSync = true; - bool subscribeRGBD = false; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize_, queueSize_); - pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); - if(pnh.hasParam("depth_cameras")) - { - ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true."); - } - pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); - if(rgbdCameras <= 0) - { - rgbdCameras = 1; - } - if(rgbdCameras > 4) - { - NODELET_FATAL("Only 4 cameras maximum supported yet."); - } - - NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); - NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_); - NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); - NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras); - - std::string subscribedTopicsMsg; - if(subscribeRGBD) - { - if(rgbdCameras >= 2) - { - rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); - rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); - if(rgbdCameras >= 3) - { - rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); - } - if(rgbdCameras >= 4) - { - rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); - } - - if(rgbdCameras == 2) - { - if(approxSync) - { - approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); - } - else - { - exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); - } - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - rgbd_image1_sub_.getTopic().c_str(), - rgbd_image2_sub_.getTopic().c_str()); - } - else if(rgbdCameras == 3) - { - if(approxSync) - { - approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_); - approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); - } - else - { - exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_); - exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); - } - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - rgbd_image1_sub_.getTopic().c_str(), - rgbd_image2_sub_.getTopic().c_str(), - rgbd_image3_sub_.getTopic().c_str()); - } - else if(rgbdCameras == 4) - { - if(approxSync) - { - approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_, - rgbd_image4_sub_); - approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); - } - else - { - exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_, - rgbd_image4_sub_); - exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); - } - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - rgbd_image1_sub_.getTopic().c_str(), - rgbd_image2_sub_.getTopic().c_str(), - rgbd_image3_sub_.getTopic().c_str(), - rgbd_image4_sub_.getTopic().c_str()); - } - } - else - { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this); - - subscribedTopicsMsg = - uFormat("\n%s subscribed to:\n %s", - getName().c_str(), - rgbdSub_.getTopic().c_str()); - } - } - else - { - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); - - if(approxSync) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - else - { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str()); - } - this->startWarningThread(subscribedTopicsMsg, approxSync); - } - - virtual void updateParameters(ParametersMap & parameters) - { - //make sure we are using Reg/Strategy=0 - ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); - if(iter != parameters.end() && iter->second.compare("0") != 0) - { - ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); - } - uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); - - int estimationType = Parameters::defaultVisEstimationType(); - Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - int rgbdCameras = 1; - bool subscribeRGBD = false; - pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); - pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); - if(subscribeRGBD && rgbdCameras> 1 && estimationType>0) - { - NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported " - "for multi-cameras) as \"subscribe_rgbd\" is " - "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.", - Parameters::kVisEstimationType().c_str(), - estimationType, - Parameters::kVisEstimationType().c_str()); - uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0")); - } - } - - void commonCallback( - const std::vector & rgbImages, - const std::vector & depthImages, - const std::vector& cameraInfos) - { - ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); - ros::Time higherStamp; - int imageWidth = rgbImages[0]->image.cols; - int imageHeight = rgbImages[0]->image.rows; - int depthWidth = depthImages[0]->image.cols; - int depthHeight = depthImages[0]->image.rows; - - UASSERT_MSG( - imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 && - imageWidth/depthWidth == imageHeight/depthHeight, - uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); - - int cameraCount = rgbImages.size(); - cv::Mat rgb; - cv::Mat depth; - pcl::PointCloud scanCloud; - std::vector cameraModels; - for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || - !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || - depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and " - "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s", - rgbImages[i]->encoding.c_str(), - depthImages[i]->encoding.c_str()); - return; - } - UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight, - uFormat("imageWidth=%d vs %d imageHeight=%d vs %d", - imageWidth, - rgbImages[i]->image.cols, - imageHeight, - rgbImages[i]->image.rows).c_str()); - UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight, - uFormat("depthWidth=%d vs %d depthHeight=%d vs %d", - depthWidth, - depthImages[i]->image.cols, - depthHeight, - depthImages[i]->image.rows).c_str()); - - ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp; - - if(i == 0) - { - higherStamp = stamp; - } - else if(stamp > higherStamp) - { - higherStamp = stamp; - } - - Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - cv_bridge::CvImageConstPtr ptrImage = rgbImages[i]; - if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 && - rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0) - { - ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8"); - } - - cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; - cv::Mat subDepth = ptrDepth->image; - - // initialize - if(rgb.empty()) - { - rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); - } - if(depth.empty()) - { - depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type()); - } - - if(ptrImage->image.type() == rgb.type()) - { - ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); - } - else - { - NODELET_ERROR("Some RGB images are not the same type!"); - return; - } - - if(subDepth.type() == depth.type()) - { - subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); - } - else - { - NODELET_ERROR("Some Depth images are not the same type!"); - return; - } - - cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform)); - } - - rtabmap::SensorData data( - rgb, - depth, - cameraModels, - 0, - rtabmap_ros::timestampFromROS(higherStamp)); - - this->processData(data, higherStamp); - } - - void callback( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo) - { - callbackCalled(); - if(!this->isPaused()) - { - std::vector imageMsgs(1); - std::vector depthMsgs(1); - std::vector infoMsgs; - imageMsgs[0] = cv_bridge::toCvShare(image); - depthMsgs[0] = cv_bridge::toCvShare(depth); - infoMsgs.push_back(*cameraInfo); - - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); - } - } - - void callbackRGBD( - const rtabmap_ros::RGBDImageConstPtr& image) - { - callbackCalled(); - if(!this->isPaused()) - { - std::vector imageMsgs(1); - std::vector depthMsgs(1); - std::vector infoMsgs; - rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); - infoMsgs.push_back(image->rgbCameraInfo); - - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); - } - } - - void callbackRGBD2( - const rtabmap_ros::RGBDImageConstPtr& image, - const rtabmap_ros::RGBDImageConstPtr& image2) - { - callbackCalled(); - if(!this->isPaused()) - { - std::vector imageMsgs(2); - std::vector depthMsgs(2); - std::vector infoMsgs; - rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); - rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); - infoMsgs.push_back(image->rgbCameraInfo); - infoMsgs.push_back(image2->rgbCameraInfo); - - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); - } - } - - void callbackRGBD3( - const rtabmap_ros::RGBDImageConstPtr& image, - const rtabmap_ros::RGBDImageConstPtr& image2, - const rtabmap_ros::RGBDImageConstPtr& image3) - { - callbackCalled(); - if(!this->isPaused()) - { - std::vector imageMsgs(3); - std::vector depthMsgs(3); - std::vector infoMsgs; - rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); - rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); - rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); - infoMsgs.push_back(image->rgbCameraInfo); - infoMsgs.push_back(image2->rgbCameraInfo); - infoMsgs.push_back(image3->rgbCameraInfo); - - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); - } - } - - void callbackRGBD4( - const rtabmap_ros::RGBDImageConstPtr& image, - const rtabmap_ros::RGBDImageConstPtr& image2, - const rtabmap_ros::RGBDImageConstPtr& image3, - const rtabmap_ros::RGBDImageConstPtr& image4) - { - callbackCalled(); - if(!this->isPaused()) - { - std::vector imageMsgs(4); - std::vector depthMsgs(4); - std::vector infoMsgs; - rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); - rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); - rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); - rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]); - infoMsgs.push_back(image->rgbCameraInfo); - infoMsgs.push_back(image2->rgbCameraInfo); - infoMsgs.push_back(image3->rgbCameraInfo); - infoMsgs.push_back(image4->rgbCameraInfo); - - this->commonCallback(imageMsgs, depthMsgs, infoMsgs); - } - } - -protected: - virtual void flushCallbacks() - { - // flush callbacks - if(approxSync_) - { - delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - if(exactSync_) - { - delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); - } - if(approxSync2_) - { - delete approxSync2_; - approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); - } - if(exactSync2_) - { - delete exactSync2_; - exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); - } - if(approxSync3_) - { - delete approxSync3_; - approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_); - approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); - } - if(exactSync3_) - { - delete exactSync3_; - exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_); - exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); - } - if(approxSync4_) - { - delete approxSync4_; - approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_, - rgbd_image4_sub_); - approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); - } - if(exactSync4_) - { - delete exactSync4_; - exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_, - rgbd_image3_sub_, - rgbd_image4_sub_); - exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); - } - } - -private: - image_transport::SubscriberFilter image_mono_sub_; - image_transport::SubscriberFilter image_depth_sub_; - message_filters::Subscriber info_sub_; - - ros::Subscriber rgbdSub_; - message_filters::Subscriber rgbd_image1_sub_; - message_filters::Subscriber rgbd_image2_sub_; - message_filters::Subscriber rgbd_image3_sub_; - message_filters::Subscriber rgbd_image4_sub_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; - typedef message_filters::sync_policies::ApproximateTime MyApproxSync2Policy; - message_filters::Synchronizer * approxSync2_; - typedef message_filters::sync_policies::ExactTime MyExactSync2Policy; - message_filters::Synchronizer * exactSync2_; - typedef message_filters::sync_policies::ApproximateTime MyApproxSync3Policy; - message_filters::Synchronizer * approxSync3_; - typedef message_filters::sync_policies::ExactTime MyExactSync3Policy; - message_filters::Synchronizer * exactSync3_; - typedef message_filters::sync_policies::ApproximateTime MyApproxSync4Policy; - message_filters::Synchronizer * approxSync4_; - typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; - message_filters::Synchronizer * exactSync4_; - int queueSize_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet); - -} diff --git a/rtabmap_demo/src/nodelets/rgbd_sync.cpp b/rtabmap_demo/src/nodelets/rgbd_sync.cpp deleted file mode 100644 index e8ddd5f..0000000 --- a/rtabmap_demo/src/nodelets/rgbd_sync.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include -#include - -#include -#include - -#include - -#include "rtabmap_ros/RGBDImage.h" - -#include "rtabmap/core/Compression.h" -#include "rtabmap/utilite/UConversion.h" - -namespace rtabmap_ros -{ - -class RGBDSync : public nodelet::Nodelet -{ -public: - RGBDSync() : - depthScale_(1.0), - warningThread_(0), - callbackCalled_(false), - approxSyncDepth_(0), - exactSyncDepth_(0) - {} - - virtual ~RGBDSync() - { - if(approxSyncDepth_) - delete approxSyncDepth_; - if(exactSyncDepth_) - delete exactSyncDepth_; - - if(warningThread_) - { - callbackCalled_=true; - warningThread_->join(); - delete warningThread_; - } - } - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - int queueSize = 10; - bool approxSync = true; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize, queueSize); - pnh.param("depth_scale", depthScale_, depthScale_); - - NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); - NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_); - - rgbdImagePub_ = nh.advertise("rgbd_image", 1); - rgbdImageCompressedPub_ = nh.advertise("rgbd_image/compressed", 1); - - if(approxSync) - { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_); - approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4)); - } - else - { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_); - exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4)); - } - - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); - cameraDepthInfoSub_.subscribe(depth_nh, "camera_info", 1); - - std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - imageSub_.getTopic().c_str(), - imageDepthSub_.getTopic().c_str(), - cameraInfoSub_.getTopic().c_str(), - cameraDepthInfoSub_.getTopic().c_str()); - - warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync)); - NODELET_INFO("%s", subscribedTopicsMsg.c_str()); - } - - void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync) - { - ros::Duration r(5.0); - while(!callbackCalled_) - { - r.sleep(); - if(!callbackCalled_) - { - ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are " - "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", - getName().c_str(), - approxSync?"":"Parameter \"approx_sync\" is false, which means that input " - "topics should have all the exact timestamp for the callback to be called.", - subscribedTopicsMsg.c_str()); - } - } - } - - void callback( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::CameraInfoConstPtr& cameraDepthInfo) - { - callbackCalled_ = true; - if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers()) - { - rtabmap_ros::RGBDImage msg; - msg.header.frame_id = cameraInfo->header.frame_id; - msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp; - msg.rgbCameraInfo = *cameraInfo; - msg.depthCameraInfo = *cameraDepthInfo; - - if(rgbdImageCompressedPub_.getNumSubscribers()) - { - rtabmap_ros::RGBDImage msgCompressed = msg; - - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); - imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG); - - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); - if(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1) - { - msgCompressed.depthCompressed.header = imageDepthPtr->header; - if(depthScale_ != 1.0) - { - msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png"); - } - else - { - msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png"); - } - msgCompressed.depthCompressed.format = "png"; - } - else - { - // Assume right stereo image - imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG); - } - - rgbdImageCompressedPub_.publish(msgCompressed); - } - - if(rgbdImagePub_.getNumSubscribers()) - { - msg.rgb = *image; - if(depthScale_ != 1.0) - { - cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth); - imageDepthPtr->image*=depthScale_; - msg.depth = *imageDepthPtr->toImageMsg(); - } - else - { - msg.depth = *depth; - } - rgbdImagePub_.publish(msg); - } - } - } - -private: - double depthScale_; - boost::thread * warningThread_; - bool callbackCalled_; - - ros::Publisher rgbdImagePub_; - ros::Publisher rgbdImageCompressedPub_; - - image_transport::SubscriberFilter imageSub_; - image_transport::SubscriberFilter imageDepthSub_; - message_filters::Subscriber cameraInfoSub_; - message_filters::Subscriber cameraDepthInfoSub_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncDepthPolicy; - message_filters::Synchronizer * approxSyncDepth_; - - typedef message_filters::sync_policies::ExactTime MyExactSyncDepthPolicy; - message_filters::Synchronizer * exactSyncDepth_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet); -} - diff --git a/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp deleted file mode 100644 index b2ef1e5..0000000 --- a/rtabmap_demo/src/nodelets/rgbdicp_odometry.cpp +++ /dev/null @@ -1,473 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 - -#include -#include -#include -#include -#include -#include - -#include "rtabmap_ros/MsgConversion.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace rtabmap; - -namespace rtabmap_ros -{ - -class RGBDICPOdometry : public rtabmap_ros::OdometryROS -{ -public: - RGBDICPOdometry() : - OdometryROS(false, true, true), - approxScanSync_(0), - exactScanSync_(0), - approxCloudSync_(0), - exactCloudSync_(0), - queueSize_(5), - scanCloudMaxPoints_(0), - scanVoxelSize_(0.0), - scanNormalK_(0), - scanNormalRadius_(0.0) - { - } - - virtual ~RGBDICPOdometry() - { - if(approxScanSync_) - { - delete approxScanSync_; - } - if(exactScanSync_) - { - delete exactScanSync_; - } - if(approxCloudSync_) - { - delete approxCloudSync_; - } - if(exactCloudSync_) - { - delete exactCloudSync_; - } - } - -private: - - virtual void onOdomInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - bool approxSync = true; - bool subscribeScanCloud = false; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize_, queueSize_); - pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); - pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - pnh.param("scan_normal_k", scanNormalK_, scanNormalK_); - if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k")) - { - ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". " - "The value is still used. Use \"scan_normal_k\" to avoid this warning."); - pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_); - } - pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_); - - NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); - NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_); - NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); - NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); - NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_); - NODELET_INFO("RGBDIcpOdometry: scan_normal_k = %d", scanNormalK_); - NODELET_INFO("RGBDIcpOdometry: scan_normal_radius = %f", scanNormalRadius_); - - ros::NodeHandle rgb_nh(nh, "rgb"); - ros::NodeHandle depth_nh(nh, "depth"); - ros::NodeHandle rgb_pnh(pnh, "rgb"); - ros::NodeHandle depth_pnh(pnh, "depth"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); - - std::string subscribedTopicsMsg; - if(subscribeScanCloud) - { - cloud_sub_.subscribe(nh, "scan_cloud", 1); - if(approxSync) - { - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); - approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); - } - else - { - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); - exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); - } - - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", - getName().c_str(), - approxSync?"approx":"exact", - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str(), - cloud_sub_.getTopic().c_str()); - } - else - { - scan_sub_.subscribe(nh, "scan", 1); - if(approxSync) - { - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); - approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); - } - else - { - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); - exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); - } - - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s", - getName().c_str(), - approxSync?"approx":"exact", - image_mono_sub_.getTopic().c_str(), - image_depth_sub_.getTopic().c_str(), - info_sub_.getTopic().c_str(), - scan_sub_.getTopic().c_str()); - } - this->startWarningThread(subscribedTopicsMsg, approxSync); - } - - virtual void updateParameters(ParametersMap & parameters) - { - //make sure we are using Reg/Strategy=0 - ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); - if(iter != parameters.end() && iter->second.compare("0") != 0) - { - ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str()); - } - uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2")); - } - - void callbackScan( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::LaserScanConstPtr& scanMsg) - { - sensor_msgs::PointCloud2ConstPtr cloudMsg; - callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); - } - - void callbackCloud( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::PointCloud2ConstPtr& cloudMsg) - { - sensor_msgs::LaserScanConstPtr scanMsg; - callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg); - } - - void callbackCommon( - const sensor_msgs::ImageConstPtr& image, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& cameraInfo, - const sensor_msgs::LaserScanConstPtr& scanMsg, - const sensor_msgs::PointCloud2ConstPtr& cloudMsg) - { - callbackCalled(); - if(!this->isPaused()) - { - if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) || - !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 " - "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s", - image->encoding.c_str(), depth->encoding.c_str()); - return; - } - - // use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node - ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp; - if(scanMsg.get() != 0) - { - if(stamp < scanMsg->header.stamp) - { - stamp = scanMsg->header.stamp; - } - } - else if(cloudMsg.get() != 0) - { - if(stamp < cloudMsg->header.stamp) - { - stamp = cloudMsg->header.stamp; - } - } - - Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) - { - rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); - cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); - cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); - - cv::Mat scan; - Transform localScanTransform = Transform::getIdentity(); - int maxLaserScans = 0; - if(scanMsg.get() != 0) - { - // make sure the frame of the laser is updated too - localScanTransform = getTransform(this->frameId(), - scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); - if(localScanTransform.isNull()) - { - ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); - return; - } - - //transform in frameId_ frame - sensor_msgs::PointCloud2 scanOut; - laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(scanOut, *pclScan); - pclScan->is_dense = true; - - maxLaserScans = (int)scanMsg->ranges.size(); - if(pclScan->size()) - { - if(scanVoxelSize_ > 0.0f) - { - float pointsBeforeFiltering = (float)pclScan->size(); - pclScan = util3d::voxelize(pclScan, scanVoxelSize_); - float ratio = float(pclScan->size()) / pointsBeforeFiltering; - maxLaserScans = int(float(maxLaserScans) * ratio); - } - if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) - { - //compute normals - pcl::PointCloud::Ptr normals; - if(scanVoxelSize_ > 0.0f) - { - normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); - } - else - { - normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_); - } - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScan2dFromPointCloud(*pclScan); - } - } - } - else if(cloudMsg.get() != 0) - { - bool containNormals = false; - if(scanVoxelSize_ == 0.0f) - { - for(unsigned int i=0; ifields.size(); ++i) - { - if(cloudMsg->fields[i].name.compare("normal_x") == 0) - { - containNormals = true; - break; - } - } - } - localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); - if(localScanTransform.isNull()) - { - ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); - return; - } - - maxLaserScans = scanCloudMaxPoints_; - if(containNormals) - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!pclScan->is_dense) - { - pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan); - } - scan = util3d::laserScanFromPointCloud(*pclScan); - } - else - { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!pclScan->is_dense) - { - pclScan = util3d::removeNaNFromPointCloud(pclScan); - } - - if(pclScan->size()) - { - if(scanVoxelSize_ > 0.0f) - { - float pointsBeforeFiltering = (float)pclScan->size(); - pclScan = util3d::voxelize(pclScan, scanVoxelSize_); - float ratio = float(pclScan->size()) / pointsBeforeFiltering; - maxLaserScans = int(float(maxLaserScans) * ratio); - } - if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) - { - //compute normals - pcl::PointCloud::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_); - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = util3d::laserScanFromPointCloud(*pclScanNormal); - } - else - { - scan = util3d::laserScanFromPointCloud(*pclScan); - } - } - } - } - - rtabmap::SensorData data( - LaserScan::backwardCompatibility(scan, - scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0, - scanMsg.get() != 0?scanMsg->range_max:0, - localScanTransform), - ptrImage->image, - ptrDepth->image, - rtabmapModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } - } - } - -protected: - virtual void flushCallbacks() - { - // flush callbacks - if(approxScanSync_) - { - delete approxScanSync_; - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); - approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); - } - if(exactScanSync_) - { - delete exactScanSync_; - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); - exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4)); - } - if(approxCloudSync_) - { - delete approxCloudSync_; - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); - approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); - } - if(exactCloudSync_) - { - delete exactCloudSync_; - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); - exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4)); - } - } - -private: - image_transport::SubscriberFilter image_mono_sub_; - image_transport::SubscriberFilter image_depth_sub_; - message_filters::Subscriber info_sub_; - message_filters::Subscriber scan_sub_; - message_filters::Subscriber cloud_sub_; - typedef message_filters::sync_policies::ApproximateTime MyApproxScanSyncPolicy; - message_filters::Synchronizer * approxScanSync_; - typedef message_filters::sync_policies::ApproximateTime MyExactScanSyncPolicy; - message_filters::Synchronizer * exactScanSync_; - typedef message_filters::sync_policies::ApproximateTime MyApproxCloudSyncPolicy; - message_filters::Synchronizer * approxCloudSync_; - typedef message_filters::sync_policies::ApproximateTime MyExactCloudSyncPolicy; - message_filters::Synchronizer * exactCloudSync_; - int queueSize_; - int scanCloudMaxPoints_; - double scanVoxelSize_; - int scanNormalK_; - double scanNormalRadius_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet); - -} diff --git a/rtabmap_demo/src/nodelets/stereo_odometry.cpp b/rtabmap_demo/src/nodelets/stereo_odometry.cpp deleted file mode 100644 index 01dc13e..0000000 --- a/rtabmap_demo/src/nodelets/stereo_odometry.cpp +++ /dev/null @@ -1,347 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "rtabmap_ros/OdometryROS.h" -#include "pluginlib/class_list_macros.h" -#include "nodelet/nodelet.h" - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include - -#include "rtabmap_ros/MsgConversion.h" - -#include -#include -#include -#include - -using namespace rtabmap; - -namespace rtabmap_ros -{ - -class StereoOdometry : public rtabmap_ros::OdometryROS -{ -public: - StereoOdometry() : - rtabmap_ros::OdometryROS(true, true, false), - approxSync_(0), - exactSync_(0), - queueSize_(5) - { - } - - virtual ~StereoOdometry() - { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - } - -private: - virtual void onOdomInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - bool approxSync = false; - bool subscribeRGBD = false; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("queue_size", queueSize_, queueSize_); - pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); - - NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); - NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_); - NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); - - std::string subscribedTopicsMsg; - if(subscribeRGBD) - { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); - - subscribedTopicsMsg = - uFormat("\n%s subscribed to:\n %s", - getName().c_str(), - rgbdSub_.getTopic().c_str()); - } - else - { - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - - if(approxSync) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - else - { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - - - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str()); - } - this->startWarningThread(subscribedTopicsMsg, approxSync); - } - - virtual void updateParameters(ParametersMap & parameters) - { - //make sure we are using Reg/Strategy=0 - ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy()); - if(iter != parameters.end() && iter->second.compare("0") != 0) - { - ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str()); - } - uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); - } - - void callback( - const sensor_msgs::ImageConstPtr& imageRectLeft, - const sensor_msgs::ImageConstPtr& imageRectRight, - const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, - const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) - { - callbackCalled(); - if(!this->isPaused()) - { - if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); - return; - } - - ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; - - Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - ros::WallTime time = ros::WallTime::now(); - - int quality = -1; - if(imageRectLeft->data.size() && imageRectRight->data.size()) - { - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform); - if(stereoModel.baseline() <= 0) - { - NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " - "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); - return; - } - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - NODELET_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - - cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); - cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); - - UTimer stepTimer; - // - UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); - rtabmap::SensorData data( - ptrImageLeft->image, - ptrImageRight->image, - stereoModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } - else - { - NODELET_WARN("Odom: input images empty?!?"); - } - } - } - - void callbackRGBD( - const rtabmap_ros::RGBDImageConstPtr& image) - { - callbackCalled(); - if(!this->isPaused()) - { - cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight; - rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight); - - if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || - !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) - { - NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)"); - return; - } - - ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; - - Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); - if(localTransform.isNull()) - { - return; - } - - ros::WallTime time = ros::WallTime::now(); - - int quality = -1; - if(!imageRectLeft->image.empty() && !imageRectRight->image.empty()) - { - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform); - if(stereoModel.baseline() <= 0) - { - NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " - "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); - return; - } - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - NODELET_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - - cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8"); - cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8"); - - UTimer stepTimer; - // - UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); - rtabmap::SensorData data( - ptrImageLeft->image, - ptrImageRight->image, - stereoModel, - 0, - rtabmap_ros::timestampFromROS(stamp)); - - this->processData(data, stamp); - } - else - { - NODELET_WARN("Odom: input images empty?!?"); - } - } - } - -protected: - virtual void flushCallbacks() - { - //flush callbacks - if(approxSync_) - { - delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - if(exactSync_) - { - delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); - } - } - -private: - image_transport::SubscriberFilter imageRectLeft_; - image_transport::SubscriberFilter imageRectRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; - ros::Subscriber rgbdSub_; - int queueSize_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet); - -} - diff --git a/rtabmap_demo/src/nodelets/stereo_throttle.cpp b/rtabmap_demo/src/nodelets/stereo_throttle.cpp deleted file mode 100644 index d5db504..0000000 --- a/rtabmap_demo/src/nodelets/stereo_throttle.cpp +++ /dev/null @@ -1,246 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "ros/ros.h" -#include "pluginlib/class_list_macros.h" -#include "nodelet/nodelet.h" - -#include -#include -#include - -#include -#include - -#include - -#include - -#include - -namespace rtabmap_ros -{ - -class StereoThrottleNodelet : public nodelet::Nodelet -{ -public: - //Constructor - StereoThrottleNodelet(): - rate_(0), - approxSync_(0), - exactSync_(0), - decimation_(1) - { - } - - virtual ~StereoThrottleNodelet() - { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - } - -private: - ros::Time last_update_; - double rate_; - virtual void onInit() - { - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - - ros::NodeHandle left_nh(nh, "left"); - ros::NodeHandle right_nh(nh, "right"); - ros::NodeHandle left_pnh(pnh, "left"); - ros::NodeHandle right_pnh(pnh, "right"); - image_transport::ImageTransport left_it(left_nh); - image_transport::ImageTransport right_it(right_nh); - image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); - image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - - int queueSize = 5; - bool approxSync = false; - pnh.param("approx_sync", approxSync, approxSync); - pnh.param("rate", rate_, rate_); - pnh.param("queue_size", queueSize, queueSize); - pnh.param("decimation", decimation_, decimation_); - ROS_ASSERT(decimation_ >= 1); - NODELET_INFO("Rate=%f Hz", rate_); - NODELET_INFO("Decimation=%d", decimation_); - NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); - - if(approxSync) - { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); - approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4)); - } - else - { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); - exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4)); - } - - imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); - imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); - - imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1); - imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1); - infoLeftPub_ = left_nh.advertise(left_nh.resolveName("camera_info")+"_throttle", 1); - infoRightPub_ = right_nh.advertise(right_nh.resolveName("camera_info")+"_throttle", 1); - }; - - void callback(const sensor_msgs::ImageConstPtr& imageLeft, - const sensor_msgs::ImageConstPtr& imageRight, - const sensor_msgs::CameraInfoConstPtr& camInfoLeft, - const sensor_msgs::CameraInfoConstPtr& camInfoRight) - { - if (rate_ > 0.0) - { - NODELET_DEBUG("update set to %f", rate_); - if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) - { - NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); - return; - } - } - else - NODELET_DEBUG("rate unset continuing"); - - last_update_ = ros::Time::now(); - - if(imageLeftPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft); - cv_bridge::CvImage out; - out.header = imagePtr->header; - out.encoding = imagePtr->encoding; - out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); - imageLeftPub_.publish(out.toImageMsg()); - } - else - { - imageLeftPub_.publish(imageLeft); - } - } - if(imageRightPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight); - cv_bridge::CvImage out; - out.header = imagePtr->header; - out.encoding = imagePtr->encoding; - out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_); - imageRightPub_.publish(out.toImageMsg()); - } - else - { - imageRightPub_.publish(imageRight); - } - } - if(infoLeftPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - sensor_msgs::CameraInfo info = *camInfoLeft; - info.height /= decimation_; - info.width /= decimation_; - info.roi.height /= decimation_; - info.roi.width /= decimation_; - info.K[2]/=float(decimation_); // cx - info.K[5]/=float(decimation_); // cy - info.K[0]/=float(decimation_); // fx - info.K[4]/=float(decimation_); // fy - info.P[2]/=float(decimation_); // cx - info.P[6]/=float(decimation_); // cy - info.P[0]/=float(decimation_); // fx - info.P[5]/=float(decimation_); // fy - info.P[3]/=float(decimation_); // Tx - infoLeftPub_.publish(info); - } - else - { - infoLeftPub_.publish(camInfoLeft); - } - } - if(infoRightPub_.getNumSubscribers()) - { - if(decimation_ > 1) - { - sensor_msgs::CameraInfo info = *camInfoRight; - info.height /= decimation_; - info.width /= decimation_; - info.roi.height /= decimation_; - info.roi.width /= decimation_; - info.K[2]/=float(decimation_); // cx - info.K[5]/=float(decimation_); // cy - info.K[0]/=float(decimation_); // fx - info.K[4]/=float(decimation_); // fy - info.P[2]/=float(decimation_); // cx - info.P[6]/=float(decimation_); // cy - info.P[0]/=float(decimation_); // fx - info.P[5]/=float(decimation_); // fy - info.P[3]/=float(decimation_); // Tx - infoRightPub_.publish(info); - } - else - { - infoRightPub_.publish(camInfoRight); - } - } - } - - image_transport::Publisher imageLeftPub_; - image_transport::Publisher imageRightPub_; - ros::Publisher infoLeftPub_; - ros::Publisher infoRightPub_; - - image_transport::SubscriberFilter imageLeft_; - image_transport::SubscriberFilter imageRight_; - message_filters::Subscriber cameraInfoLeft_; - message_filters::Subscriber cameraInfoRight_; - - typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; - message_filters::Synchronizer * approxSync_; - typedef message_filters::sync_policies::ExactTime MyExactSyncPolicy; - message_filters::Synchronizer * exactSync_; - - int decimation_; - -}; - - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoThrottleNodelet, nodelet::Nodelet); -} diff --git a/rtabmap_demo/src/nodelets/undistort_depth.cpp b/rtabmap_demo/src/nodelets/undistort_depth.cpp deleted file mode 100644 index 01a5665..0000000 --- a/rtabmap_demo/src/nodelets/undistort_depth.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 - -#include "rtabmap/core/clams/discrete_depth_distortion_model.h" -#include "rtabmap/utilite/UConversion.h" - -namespace rtabmap_ros -{ - -class UndistortDepth : public nodelet::Nodelet -{ -public: - UndistortDepth() - {} - - virtual ~UndistortDepth() - { - } - -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); - - std::string modelPath; - pnh.param("model", modelPath, modelPath); - - if(modelPath.empty()) - { - NODELET_ERROR("undistort_depth: \"model\" parameter should be set!"); - } - - model_.load(modelPath); - if(!model_.isValid()) - { - NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str()); - } - else - { - image_transport::ImageTransport it(nh); - sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this); - pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1); - } - } - - void callback(const sensor_msgs::ImageConstPtr& depth) - { - if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) - { - NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); - return; - } - - if(pub_.getNumSubscribers()) - { - if(depth->width == model_.getWidth() && depth->width == model_.getWidth()) - { - cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth); - model_.undistort(imageDepthPtr->image); - pub_.publish(imageDepthPtr->toImageMsg()); - } - else - { - NODELET_ERROR("Input depth image size (%dx%d) and distortion model " - "size (%dx%d) don't match! Cannot undistort image.", - depth->width, depth->height, - model_.getWidth(), model_.getHeight()); - } - } - } - -private: - clams::DiscreteDepthDistortionModel model_; - image_transport::Publisher pub_; - image_transport::Subscriber sub_; -}; - -PLUGINLIB_EXPORT_CLASS(rtabmap_ros::UndistortDepth, nodelet::Nodelet); -} - diff --git a/rtabmap_demo/src/rviz/InfoDisplay.cpp b/rtabmap_demo/src/rviz/InfoDisplay.cpp deleted file mode 100644 index e174f76..0000000 --- a/rtabmap_demo/src/rviz/InfoDisplay.cpp +++ /dev/null @@ -1,131 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "InfoDisplay.h" -#include "rtabmap_ros/MsgConversion.h" - -namespace rtabmap_ros -{ - -InfoDisplay::InfoDisplay() - : spinner_(1, &cbqueue_), - globalCount_(0), - localCount_(0) -{ - update_nh_.setCallbackQueue( &cbqueue_ ); -} - -InfoDisplay::~InfoDisplay() -{ - spinner_.stop(); -} - -void InfoDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - this->setStatusStd(rviz::StatusProperty::Ok, "Info", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0"); - this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0"); - - spinner_.start(); -} - -void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg ) -{ - { - boost::mutex::scoped_lock lock(info_mutex_); - if(msg->loopClosureId) - { - info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId); - globalCount_ += 1; - } - else if(msg->proximityDetectionId) - { - info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId); - localCount_ += 1; - } - else - { - info_ = ""; - } - loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform); - - rtabmap::Statistics stat; - rtabmap_ros::infoFromROS(*msg, stat); - statistics_ = stat.data(); - } - - - this->emitTimeSignal(msg->header.stamp); -} - -void InfoDisplay::update( float wall_dt, float ros_dt ) -{ - { - boost::mutex::scoped_lock lock(info_mutex_); - this->setStatusStd(rviz::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString()); - if(loopTransform_.isNull()) - { - this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); - } - else - { - float x,y,z, roll,pitch,yaw; - loopTransform_.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString()); - this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString()); - } - this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString()); - this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString()); - - for(std::map::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter) - { - this->setStatus(rviz::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second)); - } - } -} - -void InfoDisplay::reset() -{ - MFDClass::reset(); - { - boost::mutex::scoped_lock lock(info_mutex_); - info_.clear(); - globalCount_ = 0; - localCount_ = 0; - statistics_.clear(); - } -} - -} // namespace rtabmap_ros - -#include -PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/InfoDisplay.h b/rtabmap_demo/src/rviz/InfoDisplay.h deleted file mode 100644 index 7654f2a..0000000 --- a/rtabmap_demo/src/rviz/InfoDisplay.h +++ /dev/null @@ -1,70 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 INFO_DISPLAY_H -#define INFO_DISPLAY_H - -#include - -#include -#include - -namespace rtabmap_ros -{ - -class InfoDisplay: public rviz::MessageFilterDisplay -{ -Q_OBJECT -public: - InfoDisplay(); - virtual ~InfoDisplay(); - - virtual void reset(); - virtual void update( float wall_dt, float ros_dt ); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const rtabmap_ros::InfoConstPtr& cloud ); - -private: - ros::AsyncSpinner spinner_; - ros::CallbackQueue cbqueue_; - - QString info_; - int globalCount_; - int localCount_; - std::map statistics_; - rtabmap::Transform loopTransform_; - boost::mutex info_mutex_; -}; - -} // namespace rtabmap_ros - -#endif diff --git a/rtabmap_demo/src/rviz/MapCloudDisplay.cpp b/rtabmap_demo/src/rviz/MapCloudDisplay.cpp deleted file mode 100644 index e94e616..0000000 --- a/rtabmap_demo/src/rviz/MapCloudDisplay.cpp +++ /dev/null @@ -1,901 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 -#include -#include -#include "rviz/properties/bool_property.h" -#include "rviz/properties/enum_property.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/vector_property.h" - -#include - -#include "MapCloudDisplay.h" -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace rtabmap_ros -{ - - -MapCloudDisplay::CloudInfo::CloudInfo() : - manager_(0), - pose_(rtabmap::Transform::getIdentity()), - id_(0), - scene_node_(0) -{} - -MapCloudDisplay::CloudInfo::~CloudInfo() -{ - clear(); -} - -void MapCloudDisplay::CloudInfo::clear() -{ - if ( scene_node_ ) - { - manager_->destroySceneNode( scene_node_ ); - scene_node_=0; - } -} - -MapCloudDisplay::MapCloudDisplay() - : spinner_(1, &cbqueue_), - new_xyz_transformer_(false), - new_color_transformer_(false), - needs_retransform_(false), - transformer_class_loader_(NULL) -{ - //QIcon icon; - //this->setIcon(icon); - - style_property_ = new rviz::EnumProperty( "Style", "Flat Squares", - "Rendering mode to use, in order of computational complexity.", - this, SLOT( updateStyle() ), this ); - style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS ); - style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES ); - style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES ); - style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES ); - style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES ); - - point_world_size_property_ = new rviz::FloatProperty( "Size (m)", 0.01, - "Point size in meters.", - this, SLOT( updateBillboardSize() ), this ); - point_world_size_property_->setMin( 0.0001 ); - - point_pixel_size_property_ = new rviz::FloatProperty( "Size (Pixels)", 3, - "Point size in pixels.", - this, SLOT( updateBillboardSize() ), this ); - point_pixel_size_property_->setMin( 1 ); - - alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0, - "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.", - this, SLOT( updateAlpha() ), this ); - alpha_property_->setMin( 0 ); - alpha_property_->setMax( 1 ); - - xyz_transformer_property_ = new rviz::EnumProperty( "Position Transformer", "", - "Set the transformer to use to set the position of the points.", - this, SLOT( updateXyzTransformer() ), this ); - connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )), - this, SLOT( setXyzTransformerOptions( EnumProperty* ))); - - color_transformer_property_ = new rviz::EnumProperty( "Color Transformer", "", - "Set the transformer to use to set the color of the points.", - this, SLOT( updateColorTransformer() ), this ); - connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )), - this, SLOT( setColorTransformerOptions( EnumProperty* ))); - - cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4, - "Decimation of the input RGB and depth images before creating the cloud.", - this, SLOT( updateCloudParameters() ), this ); - cloud_decimation_->setMin( 1 ); - cloud_decimation_->setMax( 16 ); - - cloud_max_depth_ = new rviz::FloatProperty( "Cloud max depth (m)", 4.0f, - "Maximum depth of the generated clouds.", - this, SLOT( updateCloudParameters() ), this ); - cloud_max_depth_->setMin( 0.0f ); - cloud_max_depth_->setMax( 999.0f ); - - cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f, - "Minimum depth of the generated clouds.", - this, SLOT( updateCloudParameters() ), this ); - cloud_min_depth_->setMin( 0.0f ); - cloud_min_depth_->setMax( 999.0f ); - - cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f, - "Voxel size of the generated clouds.", - this, SLOT( updateCloudParameters() ), this ); - cloud_voxel_size_->setMin( 0.0f ); - cloud_voxel_size_->setMax( 1.0f ); - - cloud_filter_floor_height_ = new rviz::FloatProperty( "Filter floor (m)", 0.0f, - "Filter the floor up to maximum height set here " - "(only appropriate for 2D mapping).", - this, SLOT( updateCloudParameters() ), this ); - cloud_filter_floor_height_->setMin( 0.0f ); - cloud_filter_floor_height_->setMax( 999.0f ); - - cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f, - "Filter the ceiling at the specified height set here " - "(only appropriate for 2D mapping).", - this, SLOT( updateCloudParameters() ), this ); - cloud_filter_ceiling_height_->setMin( 0.0f ); - cloud_filter_ceiling_height_->setMax( 999.0f ); - - node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.0f, - "(Disabled=0) Only keep one node in the specified radius.", - this, SLOT( updateCloudParameters() ), this ); - node_filtering_radius_->setMin( 0.0f ); - node_filtering_radius_->setMax( 10.0f ); - - node_filtering_angle_ = new rviz::FloatProperty( "Node filtering angle (degrees)", 30.0f, - "(Disabled=0) Only keep one node in the specified angle in the filtering radius.", - this, SLOT( updateCloudParameters() ), this ); - node_filtering_angle_->setMin( 0.0f ); - node_filtering_angle_->setMax( 359.0f ); - - download_map_ = new rviz::BoolProperty( "Download map", false, - "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.", - this, SLOT( downloadMap() ), this ); - - download_graph_ = new rviz::BoolProperty( "Download graph", false, - "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.", - this, SLOT( downloadGraph() ), this ); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue( &cbqueue_ ); -} - -MapCloudDisplay::~MapCloudDisplay() -{ - if ( transformer_class_loader_ ) - { - delete transformer_class_loader_; - } - - spinner_.stop(); -} - -void MapCloudDisplay::loadTransformers() -{ - std::vector classes = transformer_class_loader_->getDeclaredClasses(); - std::vector::iterator ci; - - for( ci = classes.begin(); ci != classes.end(); ci++ ) - { - const std::string& lookup_name = *ci; - std::string name = transformer_class_loader_->getName( lookup_name ); - - if( transformers_.count( name ) > 0 ) - { - ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() ); - continue; - } - - rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name )); - trans->init(); - connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() )); - - TransformerInfo info; - info.transformer = trans; - info.readable_name = name; - info.lookup_name = lookup_name; - - info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_XYZ, info.xyz_props ); - setPropertiesHidden( info.xyz_props, true ); - - info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props ); - setPropertiesHidden( info.color_props, true ); - - transformers_[ name ] = info; - } -} - -void MapCloudDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - transformer_class_loader_ = new pluginlib::ClassLoader( "rviz", "rviz::PointCloudTransformer" ); - loadTransformers(); - - updateStyle(); - updateBillboardSize(); - updateAlpha(); - - spinner_.start(); -} - -void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg ) -{ - processMapData(*msg); - - this->emitTimeSignal(msg->header.stamp); -} - -void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) -{ - std::map poses; - for(unsigned int i=0; i::Ptr cloud; - pcl::IndicesPtr validIndices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData( - s.sensorData(), - cloud_decimation_->getInt(), - cloud_max_depth_->getFloat(), - cloud_min_depth_->getFloat(), - validIndices.get()); - - if(cloud_voxel_size_->getFloat()) - { - cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); - } - - if(cloud->size()) - { - if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f) - { - // convert in /odom frame - cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose()); - cloud = rtabmap::util3d::passThrough(cloud, "z", - cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f, - cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); - // convert back in /base_link frame - cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse()); - } - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*cloud, *cloudMsg); - cloudMsg->header = map.header; - - CloudInfoPtr info(new CloudInfo); - info->message_ = cloudMsg; - info->pose_ = rtabmap::Transform::getIdentity(); - info->id_ = id; - - if (transformCloud(info, true)) - { - boost::mutex::scoped_lock lock(new_clouds_mutex_); - new_cloud_infos_.erase(id); - new_cloud_infos_.insert(std::make_pair(id, info)); - } - } - } - } - } - - // Update graph - if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) - { - poses = rtabmap::graph::radiusPosesFiltering(poses, - node_filtering_radius_->getFloat(), - node_filtering_angle_->getFloat()*CV_PI/180.0); - } - - { - boost::mutex::scoped_lock lock(current_map_mutex_); - current_map_ = poses; - } -} - -void MapCloudDisplay::setPropertiesHidden( const QList& props, bool hide ) -{ - for( int i = 0; i < props.size(); i++ ) - { - props[ i ]->setHidden( hide ); - } -} - -void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ) -{ - std::string xyz_name = xyz_transformer_property_->getStdString(); - std::string color_name = color_transformer_property_->getStdString(); - - xyz_transformer_property_->clearOptions(); - color_transformer_property_->clearOptions(); - - // Get the channels that we could potentially render - typedef std::set > S_string; - S_string valid_xyz, valid_color; - bool cur_xyz_valid = false; - bool cur_color_valid = false; - bool has_rgb_transformer = false; - M_TransformerInfo::iterator trans_it = transformers_.begin(); - M_TransformerInfo::iterator trans_end = transformers_.end(); - for(;trans_it != trans_end; ++trans_it) - { - const std::string& name = trans_it->first; - const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer; - uint32_t mask = trans->supports(cloud); - if (mask & rviz::PointCloudTransformer::Support_XYZ) - { - valid_xyz.insert(std::make_pair(trans->score(cloud), name)); - if (name == xyz_name) - { - cur_xyz_valid = true; - } - xyz_transformer_property_->addOptionStd( name ); - } - - if (mask & rviz::PointCloudTransformer::Support_Color) - { - valid_color.insert(std::make_pair(trans->score(cloud), name)); - if (name == color_name) - { - cur_color_valid = true; - } - if (name == "RGB8") - { - has_rgb_transformer = true; - } - color_transformer_property_->addOptionStd( name ); - } - } - - if( !cur_xyz_valid ) - { - if( !valid_xyz.empty() ) - { - xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second ); - } - } - - if( !cur_color_valid ) - { - if( !valid_color.empty() ) - { - if (has_rgb_transformer) - { - color_transformer_property_->setStringStd( "RGB8" ); - } - else - { - color_transformer_property_->setStringStd( valid_color.rbegin()->second ); - } - } - } -} - -void MapCloudDisplay::updateAlpha() -{ - for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) - { - it->second->cloud_->setAlpha( alpha_property_->getFloat() ); - } -} - -void MapCloudDisplay::updateStyle() -{ - rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); - if( mode == rviz::PointCloud::RM_POINTS ) - { - point_world_size_property_->hide(); - point_pixel_size_property_->show(); - } - else - { - point_world_size_property_->show(); - point_pixel_size_property_->hide(); - } - for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) - { - it->second->cloud_->setRenderMode( mode ); - } - updateBillboardSize(); -} - -void MapCloudDisplay::updateBillboardSize() -{ - rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); - float size; - if( mode == rviz::PointCloud::RM_POINTS ) - { - size = point_pixel_size_property_->getFloat(); - } - else - { - size = point_world_size_property_->getFloat(); - } - for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) - { - it->second->cloud_->setDimensions( size, size, size ); - } - context_->queueRender(); -} - -void MapCloudDisplay::updateCloudParameters() -{ - // do nothing... only take effect on next generated clouds -} - -void MapCloudDisplay::downloadMap() -{ - if(download_map_->getBool()) - { - rtabmap_ros::GetMap getMapSrv; - getMapSrv.request.global = true; - getMapSrv.request.optimized = true; - getMapSrv.request.graphOnly = false; - ros::NodeHandle nh; - QMessageBox * messageBox = new QMessageBox( - QMessageBox::NoIcon, - tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()), - tr("Downloading the map... please wait (rviz could become gray!)"), - QMessageBox::NoButton); - messageBox->setAttribute(Qt::WA_DeleteOnClose, true); - messageBox->show(); - QApplication::processEvents(); - uSleep(100); // hack make sure the text in the QMessageBox is shown... - QApplication::processEvents(); - if(!ros::service::call("rtabmap/get_map_data", getMapSrv)) - { - ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. " - "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " - "to \"get_map_data\" in the launch " - "file like: .", - nh.resolveName("rtabmap/get_map_data").c_str()); - messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. " - "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " - "to \"get_map_data\" in the launch " - "file like: ."). - arg(nh.resolveName("rtabmap/get_map_data").c_str())); - } - else - { - messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...") - .arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); - QApplication::processEvents(); - this->reset(); - processMapData(getMapSrv.response.data); - messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!") - .arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); - - QTimer::singleShot(1000, messageBox, SLOT(close())); - } - download_map_->blockSignals(true); - download_map_->setBool(false); - download_map_->blockSignals(false); - } - else - { - // just stay true if double-clicked on DownloadMap property, let the - // first process above finishes - download_map_->blockSignals(true); - download_map_->setBool(true); - download_map_->blockSignals(false); - } -} - -void MapCloudDisplay::downloadGraph() -{ - if(download_graph_->getBool()) - { - rtabmap_ros::GetMap getMapSrv; - getMapSrv.request.global = true; - getMapSrv.request.optimized = true; - getMapSrv.request.graphOnly = true; - ros::NodeHandle nh; - QMessageBox * messageBox = new QMessageBox( - QMessageBox::NoIcon, - tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()), - tr("Downloading the graph... please wait (rviz could become gray!)"), - QMessageBox::NoButton); - messageBox->setAttribute(Qt::WA_DeleteOnClose, true); - messageBox->show(); - QApplication::processEvents(); - uSleep(100); // hack make sure the text in the QMessageBox is shown... - QApplication::processEvents(); - if(!ros::service::call("rtabmap/get_map_data", getMapSrv)) - { - ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. " - "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " - "to \"get_map_data\" in the launch " - "file like: .", - nh.resolveName("rtabmap/get_map_data").c_str()); - messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. " - "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service " - "to \"get_map_data\" in the launch " - "file like: ."). - arg(nh.resolveName("rtabmap/get_map_data").c_str())); - } - else - { - messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size())); - QApplication::processEvents(); - processMapData(getMapSrv.response.data); - messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size())); - - QTimer::singleShot(1000, messageBox, SLOT(close())); - } - download_graph_->blockSignals(true); - download_graph_->setBool(false); - download_graph_->blockSignals(false); - } - else - { - // just stay true if double-clicked on DownloadGraph property, let the - // first process above finishes - download_graph_->blockSignals(true); - download_graph_->setBool(true); - download_graph_->blockSignals(false); - } -} - -void MapCloudDisplay::causeRetransform() -{ - needs_retransform_ = true; -} - -void MapCloudDisplay::update( float wall_dt, float ros_dt ) -{ - rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt(); - - if (needs_retransform_) - { - retransform(); - needs_retransform_ = false; - } - - { - boost::mutex::scoped_lock lock(new_clouds_mutex_); - if( !new_cloud_infos_.empty() ) - { - float size; - if( mode == rviz::PointCloud::RM_POINTS ) { - size = point_pixel_size_property_->getFloat(); - } else { - size = point_world_size_property_->getFloat(); - } - - std::map::iterator it = new_cloud_infos_.begin(); - std::map::iterator end = new_cloud_infos_.end(); - for (; it != end; ++it) - { - CloudInfoPtr cloud_info = it->second; - - cloud_info->cloud_.reset( new rviz::PointCloud() ); - cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() ); - cloud_info->cloud_->setRenderMode( mode ); - cloud_info->cloud_->setAlpha( alpha_property_->getFloat() ); - cloud_info->cloud_->setDimensions( size, size, size ); - cloud_info->cloud_->setAutoSize(false); - - cloud_info->manager_ = context_->getSceneManager(); - - cloud_info->scene_node_ = scene_node_->createChildSceneNode(); - - cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() ); - cloud_info->scene_node_->setVisible(false); - - cloud_infos_.erase(it->first); - cloud_infos_.insert(*it); - } - - new_cloud_infos_.clear(); - } - } - - { - boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ ); - - if( lock.owns_lock() ) - { - if( new_xyz_transformer_ || new_color_transformer_ ) - { - M_TransformerInfo::iterator it = transformers_.begin(); - M_TransformerInfo::iterator end = transformers_.end(); - for (; it != end; ++it) - { - const std::string& name = it->first; - TransformerInfo& info = it->second; - - setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() ); - setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() ); - } - } - } - - new_xyz_transformer_ = false; - new_color_transformer_ = false; - } - - int totalPoints = 0; - int totalNodesShown = 0; - { - // update poses - boost::mutex::scoped_lock lock(current_map_mutex_); - if(!current_map_.empty()) - { - for (std::map::iterator it=current_map_.begin(); it != current_map_.end(); ++it) - { - std::map::iterator cloudInfoIt = cloud_infos_.find(it->first); - if(cloudInfoIt != cloud_infos_.end()) - { - totalPoints += cloudInfoIt->second->transformed_points_.size(); - cloudInfoIt->second->pose_ = it->second; - Ogre::Vector3 framePosition; - Ogre::Quaternion frameOrientation; - if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation)) - { - // Multiply frame with pose - Ogre::Matrix4 frameTransform; - frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation); - const rtabmap::Transform & p = cloudInfoIt->second->pose_; - Ogre::Matrix4 pose(p[0], p[1], p[2], p[3], - p[4], p[5], p[6], p[7], - p[8], p[9], p[10], p[11], - 0, 0, 0, 1); - frameTransform = frameTransform * pose; - Ogre::Vector3 posePosition = frameTransform.getTrans(); - Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion(); - poseOrientation.normalise(); - - cloudInfoIt->second->scene_node_->setPosition(posePosition); - cloudInfoIt->second->scene_node_->setOrientation(poseOrientation); - cloudInfoIt->second->scene_node_->setVisible(true); - ++totalNodesShown; - } - else - { - ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first); - } - - } - } - //hide not used clouds - for(std::map::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter) - { - if(current_map_.find(iter->first) == current_map_.end()) - { - iter->second->scene_node_->setVisible(false); - } - } - } - } - - this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString()); - this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString()); -} - -void MapCloudDisplay::reset() -{ - { - boost::mutex::scoped_lock lock(new_clouds_mutex_); - cloud_infos_.clear(); - new_cloud_infos_.clear(); - } - { - boost::mutex::scoped_lock lock(current_map_mutex_); - current_map_.clear(); - } - MFDClass::reset(); -} - -void MapCloudDisplay::updateXyzTransformer() -{ - boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); - if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 ) - { - return; - } - new_xyz_transformer_ = true; - causeRetransform(); -} - -void MapCloudDisplay::updateColorTransformer() -{ - boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); - if( transformers_.count( color_transformer_property_->getStdString() ) == 0 ) - { - return; - } - new_color_transformer_ = true; - causeRetransform(); -} - -void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop ) -{ - fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ ); -} - -void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop ) -{ - fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color ); -} - -void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask ) -{ - prop->clearOptions(); - - if (cloud_infos_.empty()) - { - return; - } - - boost::recursive_mutex::scoped_lock tlock(transformers_mutex_); - - const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_; - - M_TransformerInfo::iterator it = transformers_.begin(); - M_TransformerInfo::iterator end = transformers_.end(); - for (; it != end; ++it) - { - const rviz::PointCloudTransformerPtr& trans = it->second.transformer; - if ((trans->supports(msg) & mask) == mask) - { - prop->addOption( QString::fromStdString( it->first )); - } - } -} - -rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) -{ - boost::recursive_mutex::scoped_lock lock( transformers_mutex_); - M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() ); - if( it != transformers_.end() ) - { - const rviz::PointCloudTransformerPtr& trans = it->second.transformer; - if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ ) - { - return trans; - } - } - - return rviz::PointCloudTransformerPtr(); -} - -rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) -{ - boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); - M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() ); - if( it != transformers_.end() ) - { - const rviz::PointCloudTransformerPtr& trans = it->second.transformer; - if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color ) - { - return trans; - } - } - - return rviz::PointCloudTransformerPtr(); -} - - -void MapCloudDisplay::retransform() -{ - boost::recursive_mutex::scoped_lock lock(transformers_mutex_); - - for( std::map::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it ) - { - const CloudInfoPtr& cloud_info = it->second; - transformCloud(cloud_info, false); - cloud_info->cloud_->clear(); - cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size()); - } -} - -bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers) -{ - rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_; - cloud_points.clear(); - - size_t size = cloud_info->message_->width * cloud_info->message_->height; - rviz::PointCloud::Point default_pt; - default_pt.color = Ogre::ColourValue(1, 1, 1); - default_pt.position = Ogre::Vector3::ZERO; - cloud_points.resize(size, default_pt); - - { - boost::recursive_mutex::scoped_lock lock(transformers_mutex_); - if( update_transformers ) - { - updateTransformers( cloud_info->message_ ); - } - rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_); - rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_); - - if (!xyz_trans) - { - std::stringstream ss; - ss << "No position transformer available for cloud"; - this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str()); - return false; - } - - if (!color_trans) - { - std::stringstream ss; - ss << "No color transformer available for cloud"; - this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str()); - return false; - } - - xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points); - color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points); - } - - for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point) - { - if (!rviz::validateFloats(cloud_point->position)) - { - cloud_point->position.x = 999999.0f; - cloud_point->position.y = 999999.0f; - cloud_point->position.z = 999999.0f; - } - } - - return true; -} - -} // namespace rtabmap - -#include -PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapCloudDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/MapCloudDisplay.h b/rtabmap_demo/src/rviz/MapCloudDisplay.h deleted file mode 100644 index 938bef9..0000000 --- a/rtabmap_demo/src/rviz/MapCloudDisplay.h +++ /dev/null @@ -1,196 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 MAP_CLOUD_DISPLAY_H -#define MAP_CLOUD_DISPLAY_H - -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 - -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include - -#endif - -namespace rviz { -class IntProperty; -class BoolProperty; -class EnumProperty; -class FloatProperty; -class PointCloudTransformer; -typedef boost::shared_ptr PointCloudTransformerPtr; -typedef std::vector V_string; -} - -using namespace rviz; - -namespace rtabmap_ros -{ - -class PointCloudCommon; - -/** - * \class MapCloudDisplay - * \brief Displays point clouds from rtabmap::MapData - * - * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity. - * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b - * all being 8 bits. - */ -class MapCloudDisplay: public rviz::MessageFilterDisplay -{ -Q_OBJECT -public: - struct CloudInfo - { - CloudInfo(); - ~CloudInfo(); - - // clear the point cloud, but keep selection handler around - void clear(); - - Ogre::SceneManager *manager_; - - sensor_msgs::PointCloud2ConstPtr message_; - rtabmap::Transform pose_; - int id_; - - Ogre::SceneNode *scene_node_; - boost::shared_ptr cloud_; - - std::vector transformed_points_; - }; - typedef boost::shared_ptr CloudInfoPtr; - - MapCloudDisplay(); - virtual ~MapCloudDisplay(); - - virtual void reset(); - virtual void update( float wall_dt, float ros_dt ); - - rviz::FloatProperty* point_world_size_property_; - rviz::FloatProperty* point_pixel_size_property_; - rviz::FloatProperty* alpha_property_; - rviz::EnumProperty* xyz_transformer_property_; - rviz::EnumProperty* color_transformer_property_; - rviz::EnumProperty* style_property_; - rviz::IntProperty* cloud_decimation_; - rviz::FloatProperty* cloud_max_depth_; - rviz::FloatProperty* cloud_min_depth_; - rviz::FloatProperty* cloud_voxel_size_; - rviz::FloatProperty* cloud_filter_floor_height_; - rviz::FloatProperty* cloud_filter_ceiling_height_; - rviz::FloatProperty* node_filtering_radius_; - rviz::FloatProperty* node_filtering_angle_; - rviz::BoolProperty* download_map_; - rviz::BoolProperty* download_graph_; - -public Q_SLOTS: - void causeRetransform(); - -private Q_SLOTS: - void updateStyle(); - void updateBillboardSize(); - void updateAlpha(); - void updateXyzTransformer(); - void updateColorTransformer(); - void setXyzTransformerOptions( EnumProperty* prop ); - void setColorTransformerOptions( EnumProperty* prop ); - void updateCloudParameters(); - void downloadMap(); - void downloadGraph(); - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - virtual void onInitialize(); - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud ); - -private: - void processMapData(const rtabmap_ros::MapData& map); - - /** - * \brief Transforms the cloud into the correct frame, and sets up our renderable cloud - */ - bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers); - - rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); - rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); - void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ); - void retransform(); - - void loadTransformers(); - - void setPropertiesHidden( const QList& props, bool hide ); - void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask ); - -private: - ros::AsyncSpinner spinner_; - ros::CallbackQueue cbqueue_; - - std::map cloud_infos_; - - std::map new_cloud_infos_; - boost::mutex new_clouds_mutex_; - - std::map current_map_; - boost::mutex current_map_mutex_; - - struct TransformerInfo - { - rviz::PointCloudTransformerPtr transformer; - QList xyz_props; - QList color_props; - - std::string readable_name; - std::string lookup_name; - }; - typedef std::map M_TransformerInfo; - - boost::recursive_mutex transformers_mutex_; - M_TransformerInfo transformers_; - bool new_xyz_transformer_; - bool new_color_transformer_; - bool needs_retransform_; - - pluginlib::ClassLoader* transformer_class_loader_; -}; - -} // namespace rtabmap_ros - -#endif diff --git a/rtabmap_demo/src/rviz/MapGraphDisplay.cpp b/rtabmap_demo/src/rviz/MapGraphDisplay.cpp deleted file mode 100644 index f672f2e..0000000 --- a/rtabmap_demo/src/rviz/MapGraphDisplay.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "rviz/properties/color_property.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/int_property.h" - -#include "MapGraphDisplay.h" - -#include -#include - -namespace rtabmap_ros -{ - -MapGraphDisplay::MapGraphDisplay() -{ - color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue, - "Color to draw neighbor links.", this ); - color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0), - "Color to draw merged neighbor links.", this ); - color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red, - "Color to draw global loop closure links.", this ); - color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow, - "Color to draw local loop closure links.", this ); - color_user_property_ = new rviz::ColorProperty( "User", Qt::red, - "Color to draw user links.", this ); - color_virtual_property_ = new rviz::ColorProperty( "Virtual", Qt::magenta, - "Color to draw virtual links.", this ); - - alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0, - "Amount of transparency to apply to the path.", this ); -} - -MapGraphDisplay::~MapGraphDisplay() -{ - destroyObjects(); -} - -void MapGraphDisplay::onInitialize() -{ - MFDClass::onInitialize(); - destroyObjects(); -} - -void MapGraphDisplay::reset() -{ - MFDClass::reset(); - destroyObjects(); -} - -void MapGraphDisplay::destroyObjects() -{ - for(unsigned int i=0; iclear(); - scene_manager_->destroyManualObject( manual_objects_[i] ); - } - manual_objects_.clear(); -} - -void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg ) -{ - if(!(msg->poses.size() == msg->posesId.size())) - { - ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size."); - return; - } - - // Get links - std::map poses; - std::multimap links; - rtabmap::Transform mapToOdom; - rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom); - - destroyObjects(); - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) - { - ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); - } - - Ogre::Matrix4 transform( orientation ); - transform.setTrans( position ); - - if(links.size()) - { - Ogre::ColourValue color; - Ogre::ManualObject* manual_object = scene_manager_->createManualObject(); - manual_object->setDynamic( true ); - scene_node_->attachObject( manual_object ); - manual_objects_.push_back(manual_object); - - manual_object->estimateVertexCount(links.size() * 2); - manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST ); - for(std::map::iterator iter=links.begin(); iter!=links.end(); ++iter) - { - std::map::iterator poseIterFrom = poses.find(iter->second.from()); - std::map::iterator poseIterTo = poses.find(iter->second.to()); - if(poseIterFrom != poses.end() && poseIterTo != poses.end()) - { - if(iter->second.type() == rtabmap::Link::kNeighbor) - { - color = color_neighbor_property_->getOgreColor(); - } - else if(iter->second.type() == rtabmap::Link::kNeighborMerged) - { - color = color_neighbor_merged_property_->getOgreColor(); - } - else if(iter->second.type() == rtabmap::Link::kVirtualClosure) - { - color = color_virtual_property_->getOgreColor(); - } - else if(iter->second.type() == rtabmap::Link::kUserClosure) - { - color = color_user_property_->getOgreColor(); - } - else if(iter->second.type() == rtabmap::Link::kLocalSpaceClosure || iter->second.type() == rtabmap::Link::kLocalTimeClosure) - { - color = color_local_property_->getOgreColor(); - } - else - { - color = color_global_property_->getOgreColor(); - } - color.a = alpha_property_->getFloat(); - Ogre::Vector3 pos; - pos = transform * Ogre::Vector3( poseIterFrom->second.x(), poseIterFrom->second.y(), poseIterFrom->second.z() ); - manual_object->position( pos.x, pos.y, pos.z ); - manual_object->colour( color ); - pos = transform * Ogre::Vector3( poseIterTo->second.x(), poseIterTo->second.y(), poseIterTo->second.z() ); - manual_object->position( pos.x, pos.y, pos.z ); - manual_object->colour( color ); - } - } - - manual_object->end(); - } -} - -} // namespace rtabmap_ros - -#include -PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display ) diff --git a/rtabmap_demo/src/rviz/MapGraphDisplay.h b/rtabmap_demo/src/rviz/MapGraphDisplay.h deleted file mode 100644 index b17f72f..0000000 --- a/rtabmap_demo/src/rviz/MapGraphDisplay.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 MAP_GRAPH_DISPLAY_H -#define MAP_GRAPH_DISPLAY_H - -#include - -#include - -namespace Ogre -{ -class ManualObject; -} - -namespace rviz -{ -class ColorProperty; -class FloatProperty; -} - -using namespace rviz; - -namespace rtabmap_ros -{ - -/** - * \class MapGraphDisplay - * \brief Displays the graph of rtabmap::MapGraph message - */ -class MapGraphDisplay: public MessageFilterDisplay -{ -Q_OBJECT -public: - MapGraphDisplay(); - virtual ~MapGraphDisplay(); - - /** @brief Overridden from Display. */ - virtual void reset(); - -protected: - /** @brief Overridden from Display. */ - virtual void onInitialize(); - - /** @brief Overridden from MessageFilterDisplay. */ - void processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg ); - -private: - void destroyObjects(); - - std::vector manual_objects_; - - ColorProperty* color_neighbor_property_; - ColorProperty* color_neighbor_merged_property_; - ColorProperty* color_global_property_; - ColorProperty* color_local_property_; - ColorProperty* color_user_property_; - ColorProperty* color_virtual_property_; - FloatProperty* alpha_property_; -}; - -} // namespace rtabmap_ros - -#endif /* MAP_GRAPH_DISPLAY_H */ - diff --git a/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp b/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp deleted file mode 100644 index 4df70c0..0000000 --- a/rtabmap_demo/src/rviz/OrbitOrientedViewController.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 "OrbitOrientedViewController.h" - -#include -#include -#include -#include -#include -#include - -#include "rviz/properties/float_property.h" -#include "rviz/properties/vector_property.h" -#include "rviz/ogre_helpers/shape.h" - -namespace rtabmap_ros -{ - -void OrbitOrientedViewController::updateCamera() -{ - float distance = distance_property_->getFloat(); - float yaw = yaw_property_->getFloat(); - float pitch = pitch_property_->getFloat(); - - Ogre::Matrix3 rot; - reference_orientation_.ToRotationMatrix(rot); - Ogre::Radian rollTarget, pitchTarget, yawTarget; - rot.ToEulerAnglesXYZ(yawTarget, pitchTarget, rollTarget); - - yaw += rollTarget.valueRadians(); - pitch += pitchTarget.valueRadians(); - - Ogre::Vector3 focal_point = focal_point_property_->getVector(); - - float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x; - float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y; - float z = distance * sin( pitch ) + focal_point.z; - - Ogre::Vector3 pos( x, y, z ); - - camera_->setPosition(pos); - camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z); - camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos)); - - focal_shape_->setPosition( focal_point ); -} - -} - -#include -PLUGINLIB_EXPORT_CLASS( rtabmap_ros::OrbitOrientedViewController, rviz::ViewController ) diff --git a/rtabmap_demo/src/rviz/OrbitOrientedViewController.h b/rtabmap_demo/src/rviz/OrbitOrientedViewController.h deleted file mode 100644 index 9cb0594..0000000 --- a/rtabmap_demo/src/rviz/OrbitOrientedViewController.h +++ /dev/null @@ -1,51 +0,0 @@ -/* -Copyright (c) 2010-2016, 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 ORBITORIENTEDVIEWCONTROLLER_H_ -#define ORBITORIENTEDVIEWCONTROLLER_H_ - -#include "rviz/default_plugin/view_controllers/orbit_view_controller.h" - -namespace rtabmap_ros -{ - -class OrbitOrientedViewController: public rviz::OrbitViewController -{ -Q_OBJECT -public: - OrbitOrientedViewController() {} - virtual ~OrbitOrientedViewController() {} - -protected: - virtual void updateCamera(); -}; - -} - - - -#endif /* ORBITORIENTEDVIEWCONTROLLER_H_ */ From 8cc061b8890e4e824704c68d34a2ea8e3ddf2c4a Mon Sep 17 00:00:00 2001 From: anchuanxu Date: Wed, 13 Jun 2018 16:47:04 +0800 Subject: [PATCH 6/6] fix a error --- ...\205\350\257\246\347\273\206\346\255\245\351\252\244.md" | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git "a/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" "b/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" index 094f476..4df1e06 100644 --- "a/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" +++ "b/orbslam2_demo/\350\277\220\350\241\214\345\214\205\350\257\246\347\273\206\346\255\245\351\252\244.md" @@ -69,7 +69,7 @@ ippicv_linux_20151201.tgz 地址)](https://raw.githubusercontent.com/opencv/open 4. 下载orb_salm2的源码包 - 在ＲＯＳ的工作空间下　`catkin_ws/` 　输入命令： + 在ＲＯＳ的工作空间下　`catkin_ws/src/` 　输入命令： `git clone https://github.com/raulmur/ORB_SLAM2.git` 5. 构建orb_slam2 @@ -81,7 +81,7 @@ ippicv_linux_20151201.tgz 地址)](https://raw.githubusercontent.com/opencv/open ./build_ros.sh cd - grdit .bashrc + gedit .bashrc 在末尾加上：export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/【本机】/catkin_ws/src/ORB_SLAM2/Examples/ROS 6. 执行语句开始进行仿真建图 @@ -90,4 +90,4 @@ ippicv_linux_20151201.tgz 地址)](https://raw.githubusercontent.com/opencv/open 　终端2：roslaunch orbslam2_demo ros_orbslam2.launch 　 终端3：rosrun robot_sim_demo robot_keyboard_teleop.py - \ No newline at end of file +