i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/introlab/rtabmap_ros into noetic-devel

Browse Source
This commit is contained in:
matlabbe
2021-09-30 10:16:37 -04:00
parent 5597263dec a5fe8df170
commit e9ab2b3081
56 changed files with 2523 additions and 794 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/docker.yml
+53
View File
@@ -0,0 +1,53 @@
name: docker
on:
push:
branches:
- 'master'
jobs:
docker:
runs-on: ubuntu-latest
strategy:
matrix:
docker_tag: [kinetic, kinetic-latest, melodic, melodic-latest, noetic, noetic-latest]
include:
- docker_tag: kinetic
docker_path: 'kinetic'
- docker_tag: kinetic-latest
docker_path: 'kinetic/latest'
- docker_tag: melodic
docker_path: 'melodic'
- docker_tag: melodic-latest
docker_path: 'melodic/latest'
- docker_tag: noetic
docker_path: 'noetic'
- docker_tag: noetic-latest
docker_path: 'noetic/latest'
steps:
-
name: Checkout
uses: actions/checkout@v2
-
name: Set up Docker Buildx
uses: docker/setup-buildx-action@v1
-
name: Login to DockerHub
uses: docker/login-action@v1
with:
username: ${{ secrets.DOCKERHUB_USERNAME }}
password: ${{ secrets.DOCKERHUB_TOKEN }}
-
name: Build and push
uses: docker/build-push-action@v2
with:
context: ./docker/${{ matrix.docker_path }}
push: true
build-args: |
CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
cache-from: type=registry,ref=introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
cache-to: type=inline
.github/workflows/ros1.yml
+72
View File
@@ -0,0 +1,72 @@
name: ros1
on:
push:
branches: [ master ]
pull_request:
branches: [ master ]
env:
# Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
BUILD_TYPE: Release
jobs:
build:
# The CMake configure and build commands are platform agnostic and should work equally
# well on Windows or Mac. You can convert this to a matrix build if you need
# cross-platform coverage.
# See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
runs-on: ${{ matrix.os }}
strategy:
matrix:
os: [ubuntu-20.04, ubuntu-18.04]
include:
- os: ubuntu-20.04
ros_distro: 'noetic'
- os: ubuntu-18.04
ros_distro: 'melodic'
steps:
- uses: ros-tooling/[email protected]
with:
required-ros-distributions: ${{ matrix.ros_distro }}
- name: Install dependencies
run: |
sudo apt-get update
sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
- uses: actions/checkout@v2
with:
repository: 'introlab/rtabmap'
path: 'rtabmap'
- name: Setup catkin workspace
run: |
source /opt/ros/${{ matrix.ros_distro }}/setup.bash
mkdir -p ${{github.workspace}}/catkin_ws/src
cd ${{github.workspace}}/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
- uses: actions/checkout@v2
with:
path: 'catkin_ws/src/rtabmap_ros'
- name: Build latest rtabmap library
run: |
source /opt/ros/${{ matrix.ros_distro }}/setup.bash
cd ${{github.workspace}}/rtabmap
cmake -B build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/catkin_ws/devel
cmake --build build --config ${{env.BUILD_TYPE}} --target install
- name: caktkin_make
run: |
source /opt/ros/${{ matrix.ros_distro }}/setup.bash
source ${{github.workspace}}/catkin_ws/devel/setup.bash
cd ${{github.workspace}}/catkin_ws
catkin_make
catkin_make install
.travis.yml
-106
View File
@@ -1,106 +0,0 @@
sudo: true
language: cpp
compiler:
- gcc
matrix:
include:
- dist: xenial
install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update
- sudo apt-get install dpkg
- sudo apt-get -y install ros-kinetic-rtabmap-ros
- sudo apt-get -y remove ros-kinetic-rtabmap
script:
- source /opt/ros/kinetic/setup.bash
- export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
- cd ..
- mkdir -p catkin_ws/src
- cd catkin_ws/src
- catkin_init_workspace
- cd ..
- catkin_make
- cd ..
- mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git
- cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make
- make install
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
- dist: bionic
install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update
- sudo apt-get install dpkg
- sudo apt-get -y install ros-melodic-rtabmap-ros
- sudo apt-get -y remove ros-melodic-rtabmap
script:
- source /opt/ros/melodic/setup.bash
- export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
- cd ..
- mkdir -p catkin_ws/src
- cd catkin_ws/src
- catkin_init_workspace
- cd ..
- catkin_make
- cd ..
- mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git
- cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make
- make install
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
- dist: focal
install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update
- sudo apt-get install dpkg
- sudo apt-get -y install ros-noetic-rtabmap-ros
- sudo apt-get -y remove ros-noetic-rtabmap
script:
- source /opt/ros/noetic/setup.bash
- cd ..
- mkdir -p catkin_ws/src
- cd catkin_ws/src
- catkin_init_workspace
- cd ..
- catkin_make
- cd ..
- mv rtabmap_ros catkin_ws/src/.
- git clone https://github.com/introlab/rtabmap.git
- cd rtabmap
- if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
- mkdir -p build && cd build
- cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel ..
- make
- make install
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
notifications:
email:
- [email protected]
CMakeLists.txt
+25 -1
View File
@@ -31,7 +31,7 @@ find_package(find_object_2d)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.20.10 REQUIRED)
find_package(RTABMap 0.20.14 REQUIRED)
find_package(OpenCV REQUIRED)
@@ -46,6 +46,19 @@ IF(WIN32)
add_compile_options(-bigobj)
ENDIF(WIN32)
# kinetic issue, rtabmap now requires at least c++11
if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
IF(COMPILER_SUPPORTS_CXX14)
set(CMAKE_CXX_STANDARD 14)
ELSEIF(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_STANDARD 11)
ENDIF()
endif()
option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support" OFF)
option(RTABMAP_SYNC_USER_DATA "Build with input user data support" OFF)
MESSAGE(STATUS "RTABMAP_SYNC_MULTI_RGBD = ${RTABMAP_SYNC_MULTI_RGBD}")
@@ -103,6 +116,7 @@ add_message_files(
Point3f.msg
Goal.msg
RGBDImage.msg
RGBDImages.msg
UserData.msg
GPS.msg
Path.msg
@@ -124,6 +138,9 @@ add_message_files(
GetNodeData.srv
GetNodesInRadius.srv
LoadDatabase.srv
DetectMoreLoopClosures.srv
GlobalBundleAdjustment.srv
CleanupLocalGrids.srv
)
## Generate added messages and services with any dependencies listed here
@@ -200,6 +217,7 @@ SET(rtabmap_sync_lib_src
src/impl/CommonDataSubscriberStereo.cpp
src/impl/CommonDataSubscriberRGB.cpp
src/impl/CommonDataSubscriberRGBD.cpp
src/impl/CommonDataSubscriberRGBDX.cpp
src/impl/CommonDataSubscriberScan.cpp
src/impl/CommonDataSubscriberOdom.cpp
src/CoreWrapper.cpp # we put CoreWrapper here instead of plugins lib to avoid long compilation time on plugins lib
@@ -240,6 +258,7 @@ SET(rtabmap_plugins_lib_src
src/nodelets/point_cloud_assembler.cpp
src/nodelets/undistort_depth.cpp
src/nodelets/imu_to_tf.cpp
src/nodelets/rgbdx_sync.cpp
)
IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
@@ -322,6 +341,10 @@ add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp)
target_link_libraries(rtabmap_rgbd_sync ${Libraries})
set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync")
add_executable(rtabmap_rgbdx_sync src/RGBDXSyncNode.cpp)
target_link_libraries(rtabmap_rgbdx_sync ${Libraries})
set_target_properties(rtabmap_rgbdx_sync PROPERTIES OUTPUT_NAME "rgbdx_sync")
add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp)
target_link_libraries(rtabmap_stereo_sync ${Libraries})
set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync")
@@ -559,6 +582,7 @@ install(TARGETS
rtabmap_point_cloud_assembler
rtabmap_camera
rtabmap_rgbd_sync
rtabmap_rgbdx_sync
rtabmap_rgbd_relay
rtabmap_wifi_signal_sub
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
README.md
+2 -2
View File
@@ -1,5 +1,5 @@
rtabmap_ros [![Build Status](https://travis-ci.org/introlab/rtabmap_ros.svg?branch=master)](https://travis-ci.org/introlab/rtabmap_ros)
===========
rtabmap_ros [![Build Status](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml) [![docker](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml)
=======
RTAB-Map's ROS package.
docker/README.md
+1 -1
View File
@@ -7,7 +7,7 @@
melodic, melodic-latest
noetic, noetic-latest
```
* The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source. The other images have the same version than the binaries released on ROS.
* The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including GTSAM and libpointmatcher dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS.
* The following example show how to launch a camera on host computer and run our pre-built rtabmap container. All examples from [RGB-D tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) and [stereo tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping) should work using rtabmap from the container instead. Launch camera on host computer (set ROS_IP as the IP used for docker):
docker/kinetic/Dockerfile
+1
View File
@@ -1,5 +1,6 @@
FROM ros:kinetic-perception
# install rtabmap packages
ARG CACHE_DATE=2016-01-01
RUN apt-get update && apt-get install -y \
ros-kinetic-rtabmap \
ros-kinetic-rtabmap-ros \
docker/melodic/Dockerfile
+1
View File
@@ -1,5 +1,6 @@
FROM ros:melodic-perception
# install rtabmap packages
ARG CACHE_DATE=2016-01-01
RUN apt-get update && apt-get install -y \
ros-melodic-rtabmap \
ros-melodic-rtabmap-ros \
docker/noetic/Dockerfile
+1
View File
@@ -1,5 +1,6 @@
FROM ros:noetic-perception
# install rtabmap packages
ARG CACHE_DATE=2016-01-01
RUN apt-get update && apt-get install -y \
ros-noetic-rtabmap \
ros-noetic-rtabmap-ros \
include/rtabmap_ros/CommonDataSubscriber.h
+44
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/Odometry.h>
#include <rtabmap_ros/RGBDImage.h>
#include <rtabmap_ros/RGBDImages.h>
#include <rtabmap_ros/UserData.h>
#include <rtabmap_ros/OdomInfo.h>
#include <rtabmap_ros/ScanDescriptor.h>
@@ -176,6 +177,17 @@ private:
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
void setupRGBDXCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
#ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks(
ros::NodeHandle & nh,
@@ -278,6 +290,8 @@ private:
//for rgbd callback
ros::Subscriber rgbdSub_;
std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
ros::Subscriber rgbdXSubOnly_;
message_filters::Subscriber<rtabmap_ros::RGBDImages> rgbdXSub_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
@@ -419,6 +433,36 @@ private:
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
#endif
// X RGBD
void rgbdXCallback(const rtabmap_ros::RGBDImagesConstPtr&);
DATA_SYNCS2(rgbdXScan2d, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS2(rgbdXScan3d, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2)
DATA_SYNCS2(rgbdXScanDesc, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS2(rgbdXInfo, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
// X RGBD + Odom
DATA_SYNCS2(rgbdXOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImages);
DATA_SYNCS3(rgbdXOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdXOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdXOdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS3(rgbdXOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// X RGBD + User Data
DATA_SYNCS2(rgbdXData, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
DATA_SYNCS3(rgbdXDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdXDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdXDataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS3(rgbdXDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
// X RGBD + Odom + User Data
DATA_SYNCS3(rgbdXOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
DATA_SYNCS4(rgbdXOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS4(rgbdXOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS4(rgbdXOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
include/rtabmap_ros/CommonDataSubscriberDefines.h
+21 -21
View File
@@ -105,18 +105,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
// Sync declarations
#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s", \
name_.c_str(), \
@@ -124,18 +124,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB0.getTopic().c_str(), \
SUB1.getTopic().c_str());
#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -144,18 +144,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB1.getTopic().c_str(), \
SUB2.getTopic().c_str());
#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -165,18 +165,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB2.getTopic().c_str(), \
SUB3.getTopic().c_str());
#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -187,18 +187,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB3.getTopic().c_str(), \
SUB4.getTopic().c_str());
#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -210,18 +210,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB4.getTopic().c_str(), \
SUB5.getTopic().c_str());
#define SYNC_DECL7(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -234,18 +234,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB5.getTopic().c_str(), \
SUB6.getTopic().c_str());
#define SYNC_DECL8(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
include/rtabmap_ros/CoreWrapper.h
+16 -1
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include "std_msgs/Int32MultiArray.h"
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/GetPlan.h>
@@ -63,6 +64,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/AddLink.h"
#include "rtabmap_ros/GetNodesInRadius.h"
#include "rtabmap_ros/LoadDatabase.h"
#include "rtabmap_ros/DetectMoreLoopClosures.h"
#include "rtabmap_ros/GlobalBundleAdjustment.h"
#include "rtabmap_ros/CleanupLocalGrids.h"
#include "MapsManager.h"
@@ -162,6 +166,7 @@ private:
void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
#endif
void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg);
void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
@@ -196,6 +201,9 @@ private:
bool loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request&, rtabmap_ros::LoadDatabase::Response&);
bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool detectMoreLoopClosuresCallback(rtabmap_ros::DetectMoreLoopClosures::Request&, rtabmap_ros::DetectMoreLoopClosures::Response&);
bool globalBundleAdjustmentCallback(rtabmap_ros::GlobalBundleAdjustment::Request&, rtabmap_ros::GlobalBundleAdjustment::Response&);
bool cleanupLocalGridsCallback(rtabmap_ros::CleanupLocalGrids::Request&, rtabmap_ros::CleanupLocalGrids::Response&);
bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -235,6 +243,7 @@ private:
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
void publishLocalPath(const ros::Time & stamp);
void publishGlobalPath(const ros::Time & stamp);
void republishMaps();
private:
rtabmap::Rtabmap rtabmap_;
@@ -312,6 +321,9 @@ private:
ros::ServiceServer loadDatabaseSrv_;
ros::ServiceServer triggerNewMapSrv_;
ros::ServiceServer backupDatabase_;
ros::ServiceServer detectMoreLoopClosuresSrv_;
ros::ServiceServer globalBundleAdjustmentSrv_;
ros::ServiceServer cleanupLocalGridsSrv_;
ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer setLogDebugSrv_;
@@ -356,10 +368,11 @@ private:
ros::Subscriber gpsFixAsyncSub_;
rtabmap::GPS gps_;
ros::Subscriber tagDetectionsSub_;
std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_;
std::string imuFrameId_;
ros::Subscriber republishNodeDataSub_;
ros::Subscriber interOdomSub_;
std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
@@ -377,6 +390,8 @@ private:
bool alreadyRectifiedImages_;
bool twoDMapping_;
ros::Time previousStamp_;
std::set<int> nodesToRepublish_;
int maxNodesRepublished_;
};
}
include/rtabmap_ros/GuiWrapper.h
+1
View File
@@ -117,6 +117,7 @@ private:
rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_;
double lastOdomInfoUpdateTime_;
std::string rtabmapNodeName_;
// odometry subscription stuffs
std::string frameId_;
include/rtabmap_ros/MapsManager.h
-1
View File
@@ -128,7 +128,6 @@ private:
rtabmap::OctoMap * octomap_;
int octomapTreeDepth_;
bool octomap_frontier_flood_fill_;
bool octomapUpdated_;
rtabmap::ParametersMap parameters_;
include/rtabmap_ros/MsgConversion.h
+3 -2
View File
@@ -74,6 +74,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
void toCvShare(const rtabmap_ros::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId);
rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);
@@ -169,13 +170,13 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg, bool ignoreData = false);
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
rtabmap::Landmarks landmarksFromROS(
const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
const std::string & frameId,
const std::string & odomFrameId,
const ros::Time & odomStamp,
include/rtabmap_ros/PreferencesDialogROS.h
+2 -1
View File
@@ -36,7 +36,7 @@ using namespace rtabmap;
class PreferencesDialogROS : public PreferencesDialog
{
public:
PreferencesDialogROS(const QString & configFile);
PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName);
virtual ~PreferencesDialogROS();
virtual QString getIniFilePath() const;
@@ -52,6 +52,7 @@ protected:
private:
QString configFile_;
std::string rtabmapNodeName_;
};
#endif /* PREFERENCESDIALOGROS_H_ */
launch/demo/demo_husky.launch
+35 -7
View File
@@ -6,6 +6,8 @@
$ roslaunch husky_gazebo husky_playpen.launch realsense_enabled:=true
$ roslaunch husky_viz view_robot.launch
For ICP odometry examples, rtabmap should be built with libpointmatcher.
Examples:
1) 6DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false
@@ -16,15 +18,30 @@
3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
4) 3DoF mapping with 2D LiDAR
4) 3DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true
5) 3DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true
6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
7) 3DoF mapping with 2D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
5) 3DoF mapping with 2D LiDAR and RGB-D camera
8) 3DoF mapping with 2D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
6) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
Issues:
When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
"Extrapolation Error: Lookup would require extrapolation into the future. Requested
@@ -42,6 +59,7 @@
<arg name="lidar3d" default="false"/>
<arg name="lidar3d_ray_tracing" default="true"/>
<arg name="slam2d" default="true"/>
<arg name="depth_from_lidar" default="false"/>
<arg if="$(arg lidar3d)" name="cell_size" default="0.2"/>
@@ -66,20 +84,30 @@
<!-- 2D LiDAR -->
<arg name="subscribe_scan" value="$(arg lidar2d)" />
<arg name="scan_topic" value="/scan" />
<arg if="$(arg lidar2d)" name="scan_topic" value="/scan" />
<arg unless="$(arg lidar2d)" name="scan_topic" value="/scan_not_used" />
<!-- 3D LiDAR -->
<arg name="subscribe_scan_cloud" value="$(arg lidar3d)" />
<arg name="scan_cloud_topic" value="/velodyne_points" />
<arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
<arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
<!-- If camera is used -->
<arg name="depth" value="$(arg camera)" />
<arg name="rgbd_sync" value="$(arg camera)" />
<arg name="depth" value="$(eval camera and not depth_from_lidar)" />
<arg name="subscribe_rgb" value="$(eval camera)" />
<arg name="rgbd_sync" value="$(eval camera and not depth_from_lidar)" />
<arg name="rgb_topic" value="/realsense/color/image_raw" />
<arg name="camera_info_topic" value="/realsense/color/camera_info" />
<arg name="depth_topic" value="/realsense/depth/image_rect_raw" />
<arg name="approx_rgbd_sync" value="false" />
<!-- If depth generated from lidar projection (in case we have only a single RGB camera with a 3D lidar) -->
<arg name="gen_depth" value="$(arg depth_from_lidar)" />
<arg name="gen_depth_decimation" value="4" />
<arg name="gen_depth_fill_holes_size" value="3" />
<arg name="gen_depth_fill_iterations" value="1" />
<arg name="gen_depth_fill_holes_error" value="0.3" />
<!-- If icp_odometry is used -->
<arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
<arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
launch/demo/demo_robot_mapping.launch
+1 -1
View File
@@ -56,7 +56,7 @@
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/> <!-- minimum scan overlap to accept loop closure -->
<param name="Icp/PM" type="string" value="false"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.05"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.15"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<!-- localization mode -->
launch/rtabmap.launch
+48 -14
View File
@@ -18,6 +18,7 @@
<arg name="stereo" default="false"/>
<arg if="$(arg stereo)" name="depth" default="false"/>
<arg unless="$(arg stereo)" name="depth" default="true"/>
<arg name="subscribe_rgb" default="$(arg depth)"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
@@ -50,6 +51,7 @@
<arg name="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
<arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
<arg unless="$(arg gdb)" name="launch_prefix" default=""/>
<arg name="clear_params" default="true"/>
<arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<arg name="publish_tf_map" default="true"/>
@@ -83,9 +85,13 @@
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="gen_cloud" default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
<arg name="gen_cloud_decimation" default="4"/>
<arg name="gen_cloud_voxel" default="0.05"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/>
<arg name="subscribe_scan_cloud" default="$(arg gen_cloud)"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="subscribe_scan_descriptor" default="false"/>
<arg name="scan_descriptor_topic" default="/scan_descriptor"/>
@@ -93,6 +99,12 @@
<arg name="scan_cloud_filtered" default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
<arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
<arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
<arg name="gen_depth_decimation" default="1" />
<arg name="gen_depth_fill_holes_size" default="0" />
<arg name="gen_depth_fill_iterations" default="1" />
<arg name="gen_depth_fill_holes_error" default="0.1" />
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
@@ -112,9 +124,12 @@
<arg name="use_odom_features" default="false"/>
<arg name="scan_cloud_assembling" default="false"/>
<arg name="scan_cloud_assembling_time" default="1"/>
<arg name="scan_cloud_assembling_time" default="1"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_max_clouds" default="0"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_fixed_frame" default=""/>
<arg name="scan_cloud_assembling_voxel_size" default="0.05"/>
<arg name="scan_cloud_assembling_range_min" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_range_max" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_radius" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_min_neighbors" default="5"/>
@@ -152,7 +167,7 @@
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -172,7 +187,7 @@
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -186,7 +201,7 @@
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
@@ -195,12 +210,22 @@
</group>
</group>
<node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="$(arg scan_cloud_topic)" />
<param name="decimation" type="double" value="$(arg gen_cloud_decimation)"/>
<param name="voxel_size" type="double" value="$(arg gen_cloud_voxel)"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<!-- Visual odometry -->
<group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)">
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -228,7 +253,7 @@
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -259,7 +284,7 @@
</group>
<!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
@@ -282,24 +307,27 @@
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
<param name="max_clouds" type="int" value="$(arg scan_cloud_assembling_max_clouds)"/>
<param name="fixed_frame_id" type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
<param name="voxel_size" type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
<param name="range_min" type="double" value="$(arg scan_cloud_assembling_range_min)"/>
<param name="range_max" type="double" value="$(arg scan_cloud_assembling_range_max)"/>
<param name="noise_radius" type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
<param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
@@ -324,9 +352,14 @@
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="gen_depth" type="bool" value="$(arg gen_depth)" />
<param name="gen_depth_decimation" type="int" value="$(arg gen_depth_decimation)" />
<param name="gen_depth_fill_holes_size" type="int" value="$(arg gen_depth_fill_holes_size)" />
<param name="gen_depth_fill_iterations" type="int" value="$(arg gen_depth_fill_iterations)" />
<param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
@@ -359,9 +392,10 @@
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param unless="$(arg icp_odometry)" name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
@@ -399,7 +433,7 @@
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
launch/tests/test_ouster_gen2.launch
+43 -7
View File
@@ -4,23 +4,48 @@
<!--
Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
RVIZ: Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
PTP mode (synchronize timestamp with host computer time)
* Install:
$ sudo apt install linuxptp httpie
$ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
* Running:
(replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
(replace "eth0" by the network interface used to communicate with ouster)
$ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
$ sudo ptp4l -i eth0 -m -f ~/os.conf -S
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-->
<arg name="use_sim_time" default="false"/>
<!-- Required: -->
<arg name="sensor_hostname"/>
<arg name="udp_dest"/>
<arg unless="$(arg use_sim_time)" name="sensor_hostname"/>
<arg unless="$(arg use_sim_time)" name="udp_dest"/>
<arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="use_sim_time" default="false"/>
<arg name="assemble" default="false"/>
<arg name="ptp" default="false"/> <!-- See comments in header to start before launching the launch -->
<arg name="distortion_correction" default="false"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 -->
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster -->
@@ -30,6 +55,8 @@
<arg name="image" value="true"/>
<arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
<arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
<arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
<arg if="$(arg distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
@@ -51,12 +78,12 @@
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param name="wait_imu_to_init" type="bool" value="true"/>
@@ -88,11 +115,13 @@
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
<param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -128,6 +157,13 @@
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="/os_cloud_node/points"/>
<remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" />
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
launch/tests/test_velodyne.launch
+40 -18
View File
@@ -14,14 +14,26 @@
<arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
<arg name="imu_topic" default="/imu/data"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="organize_cloud" default="false"/>
<arg name="scan_topic" default="/velodyne_points"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="frame_id" default="velodyne"/>
<arg name="queue_size" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
<include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, kitti) -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
@@ -33,9 +45,11 @@
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/velodyne_points"/>
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
@@ -45,23 +59,29 @@
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
@@ -70,6 +90,7 @@
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<remap from="imu" to="$(arg imu_topic)"/>
@@ -84,29 +105,27 @@
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/VoxelSize" type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
@@ -115,15 +134,18 @@
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/velodyne_points"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<remap from="cloud" to="/velodyne_points"/>
<remap from="cloud" to="$(arg scan_topic)"/>
<remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
<param name="fixed_frame_id" type="string" value="" />
<param name="voxel_size" type="double" value="$(arg resolution)" />
<param name="queue_size" type="int" value="$(arg queue_size)" />
</node>
</group>
msg/OdomInfo.msg
+2 -4
View File
@@ -47,10 +47,8 @@ int32[] wordInliers
int32[] localMapKeys
Point3f[] localMapValues
# compressed local scan map data
# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
uint8[] localScanMap
int32 localScanMapFormat
# local scan map data
sensor_msgs/PointCloud2 localScanMap
# F2F odometry
# std::vector<cv::Point2f> refCorners;
msg/RGBDImages.msg
+4
View File
@@ -0,0 +1,4 @@
Header header
rtabmap_ros/RGBDImage[] rgbd_images
nodelet_plugins.xml
+8
View File
@@ -128,6 +128,14 @@
</description>
</class>
<class name="rtabmap_ros/rgbdx_sync"
type="rtabmap_ros::RGBDXSync"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap_ros/rgbd_relay"
type="rtabmap_ros::RGBDRelay"
base_class_type="nodelet::Nodelet">
package.xml
+4 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.20.10</version>
<version>0.20.14</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
@@ -44,6 +44,7 @@
<build_depend>find_object_2d</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>apriltag_ros</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>roscpp</run_depend>
@@ -57,6 +58,7 @@
<run_depend>visualization_msgs</run_depend>
<run_depend>rosgraph_msgs</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>theora_image_transport</run_depend>
<run_depend>compressed_depth_image_transport</run_depend>
<run_depend>compressed_image_transport</run_depend>
<run_depend>tf</run_depend>
@@ -79,6 +81,7 @@
<run_depend>find_object_2d</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>apriltag_ros</run_depend>
<build_depend>libpcl-all-dev</build_depend>
scripts/yaml_to_camera_info.py
+1 -1
View File
@@ -33,7 +33,7 @@ if __name__ == "__main__":
yaml_path = rospy.get_param('~yaml_path', '')
if not yaml_path:
print 'yaml_path parameter should be set to path of the calibration file!'
print('yaml_path parameter should be set to path of the calibration file!')
sys.exit(1)
frameId = rospy.get_param('~frame_id', '')
src/CommonDataSubscriber.cpp
+84 -26
View File
@@ -163,6 +163,34 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbdOdomDataScanDesc),
SYNC_INIT(rgbdOdomDataInfo),
#endif
// X RGBD
SYNC_INIT(rgbdXScan2d),
SYNC_INIT(rgbdXScan3d),
SYNC_INIT(rgbdXScanDesc),
SYNC_INIT(rgbdXInfo),
// X RGBD + Odom
SYNC_INIT(rgbdXOdom),
SYNC_INIT(rgbdXOdomScan2d),
SYNC_INIT(rgbdXOdomScan3d),
SYNC_INIT(rgbdXOdomScanDesc),
SYNC_INIT(rgbdXOdomInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// X RGBD + User Data
SYNC_INIT(rgbdXData),
SYNC_INIT(rgbdXDataScan2d),
SYNC_INIT(rgbdXDataScan3d),
SYNC_INIT(rgbdXDataScanDesc),
SYNC_INIT(rgbdXDataInfo),
// X RGBD + Odom + User Data
SYNC_INIT(rgbdXOdomData),
SYNC_INIT(rgbdXOdomDataScan2d),
SYNC_INIT(rgbdXOdomDataScan3d),
SYNC_INIT(rgbdXOdomDataScanDesc),
SYNC_INIT(rgbdXOdomDataInfo),
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
@@ -438,11 +466,6 @@ void CommonDataSubscriber::setupCallbacks(
pnh.param("approx_sync", approxSync_, approxSync_);
}
if(rgbdCameras <= 0 && subscribedToRGBD_)
{
rgbdCameras = 1;
}
ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
@@ -496,9 +519,30 @@ void CommonDataSubscriber::setupCallbacks(
}
else if(subscribedToRGBD_)
{
#ifdef RTABMAP_SYNC_MULTI_RGBD
if(rgbdCameras == 6)
if(rgbdCameras == 0)
{
setupRGBDXCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeScanDesc,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
#ifdef RTABMAP_SYNC_MULTI_RGBD
else if(rgbdCameras >= 6)
{
if(rgbdCameras > 6)
{
ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
"rgbd_cameras=0 to use RGBDImages interface instead, then "
"synchronize RGBDImage topics yourself.", rgbdCameras);
}
setupRGBD6Callbacks(
nh,
pnh,
@@ -570,7 +614,10 @@ void CommonDataSubscriber::setupCallbacks(
#else
if(rgbdCameras>1)
{
ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has been built without RTABMAP_SYNC_MULTI_RGBD option)");
ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
"been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
"use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
"but you will have to synchronize RGBDImage topics yourself.");
}
#endif
else
@@ -749,6 +796,35 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbdOdomDataInfo);
#endif
// X RGBD
SYNC_DEL(rgbdXScan2d);
SYNC_DEL(rgbdXScan3d);
SYNC_DEL(rgbdXScanDesc);
SYNC_DEL(rgbdXInfo);
// X RGBD + Odom
SYNC_DEL(rgbdXOdom);
SYNC_DEL(rgbdXOdomScan2d);
SYNC_DEL(rgbdXOdomScan3d);
SYNC_DEL(rgbdXOdomScanDesc);
SYNC_DEL(rgbdXOdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// X RGBD + User Data
SYNC_DEL(rgbdXData);
SYNC_DEL(rgbdXDataScan2d);
SYNC_DEL(rgbdXDataScan3d);
SYNC_DEL(rgbdXDataScanDesc);
SYNC_DEL(rgbdXDataInfo);
// X RGBD + Odom + User Data
SYNC_DEL(rgbdXOdomData);
SYNC_DEL(rgbdXOdomDataScan2d);
SYNC_DEL(rgbdXOdomDataScan3d);
SYNC_DEL(rgbdXOdomDataScanDesc);
SYNC_DEL(rgbdXOdomDataInfo);
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
SYNC_DEL(rgbd2);
@@ -912,24 +988,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
delete rgbdSubs_[i];
}
rgbdSubs_.clear();
//clear params
ros::NodeHandle pnh("~");
pnh.deleteParam("subscribe_depth");
pnh.deleteParam("subscribe_laserScan");
pnh.deleteParam("subscribe_scan");
pnh.deleteParam("subscribe_scan_cloud");
pnh.deleteParam("subscribe_stereo");
pnh.deleteParam("subscribe_rgb");
pnh.deleteParam("subscribe_rgbd");
pnh.deleteParam("subscribe_odom_info");
pnh.deleteParam("subscribe_user_data");
pnh.deleteParam("odom_frame_id");
pnh.deleteParam("rgbd_cameras");
pnh.deleteParam("depth_cameras");
pnh.deleteParam("queue_size");
pnh.deleteParam("approx_sync");
pnh.deleteParam("stereo_approx_sync");
}
void CommonDataSubscriber::warningLoop()
src/CoreWrapper.cpp
+309 -31
View File
@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Registration.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
@@ -124,7 +125,8 @@ CoreWrapper::CoreWrapper() :
alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
twoDMapping_(Parameters::defaultRegForce3DoF()),
previousStamp_(0),
mbClient_(0)
mbClient_(0),
maxNodesRepublished_(2)
{
char * rosHomePath = getenv("ROS_HOME");
std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
@@ -187,6 +189,7 @@ void CoreWrapper::onInit()
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
pnh.param("max_nodes_republished", maxNodesRepublished_, maxNodesRepublished_);
pnh.param("gen_scan", genScan_, genScan_);
pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_);
@@ -610,6 +613,7 @@ void CoreWrapper::onInit()
Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
}
paused_ = pnh.param("is_rtabmap_paused", paused_);
if(paused_)
{
NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -639,7 +643,7 @@ void CoreWrapper::onInit()
if(rtabmap_.getMemory())
{
if(useSavedMap_ && !rtabmap_.getMemory()->isIncremental())
if(useSavedMap_)
{
float xMin, yMin, gridCellSize;
cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
@@ -680,6 +684,9 @@ void CoreWrapper::onInit()
loadDatabaseSrv_ = nh.advertiseService("load_database", &CoreWrapper::loadDatabaseCallback, this);
triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
detectMoreLoopClosuresSrv_ = nh.advertiseService("detect_more_loop_closures", &CoreWrapper::detectMoreLoopClosuresCallback, this);
globalBundleAdjustmentSrv_ = nh.advertiseService("global_bundle_adjustment", &CoreWrapper::globalBundleAdjustmentCallback, this);
cleanupLocalGridsSrv_ = nh.advertiseService("cleanup_local_grids", &CoreWrapper::cleanupLocalGridsCallback, this);
setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
getNodeDataSrv_ = nh.advertiseService("get_node_data", &CoreWrapper::getNodeDataCallback, this);
@@ -801,11 +808,10 @@ void CoreWrapper::onInit()
}
}
// set public parameters
nh.setParam("is_rtabmap_paused", paused_);
// set private parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
nh.setParam(iter->first, iter->second);
pnh.setParam(iter->first, iter->second);
}
userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
@@ -815,6 +821,7 @@ void CoreWrapper::onInit()
tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
#endif
imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
}
CoreWrapper::~CoreWrapper()
@@ -828,13 +835,6 @@ CoreWrapper::~CoreWrapper()
this->saveParameters(configPath_);
ros::NodeHandle nh;
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
nh.deleteParam(iter->first);
}
nh.deleteParam("is_rtabmap_paused");
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
if(rtabmap_.getMemory())
{
@@ -1167,7 +1167,7 @@ void CoreWrapper::commonDepthCallback(
return;
}
}
else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomUpdate(odomMsg, imageMsgs[0]->header.stamp))
else if(cameraInfoMsgs.size() == 0 || !odomUpdate(odomMsg, cameraInfoMsgs[0].header.stamp))
{
return;
}
@@ -1186,7 +1186,7 @@ void CoreWrapper::commonDepthCallback(
return;
}
}
else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomTFUpdate(imageMsgs[0]->header.stamp))
else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp))
{
return;
}
@@ -1382,7 +1382,7 @@ void CoreWrapper::commonDepthCallbackImpl(
rgb,
depth,
cameraModels,
lastPoseIntermediate_?-1:imageMsgs[0]->header.seq,
lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1912,9 +1912,9 @@ void CoreWrapper::process(
else if(twoDMapping_)
{
// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
}
SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
@@ -2088,9 +2088,9 @@ void CoreWrapper::process(
else if(twoDMapping_)
{
// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
}
std::map<std::string, float> externalStats;
@@ -2454,7 +2454,9 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
warned = true;
}
}
uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p));
uInsert(tags_,
std::make_pair(tagDetections.detections[i].id[0],
std::make_pair(p, tagDetections.detections[i].size.size()==1?(float)tagDetections.detections[i].size[0]:0.0f)));
}
}
}
@@ -2496,6 +2498,26 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
}
}
void CoreWrapper::republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg)
{
if(maxNodesRepublished_>0)
{
nodesToRepublish_.insert(msg->data.begin(), msg->data.end());
}
else
{
static bool warned = false;
if(!warned)
{
NODELET_WARN("A node is requesting some node data "
"to be republished after the next update, "
"but parameter \"max_nodes_republished\" is not over 0, "
"ignoring the call. This warning is only printed once.");
warned = true;
}
}
}
void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
{
if(!paused_)
@@ -2717,29 +2739,29 @@ void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(nh.getParam(iter->first, vStr))
if(pnh.getParam(iter->first, vStr))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(nh.getParam(iter->first, vBool))
else if(pnh.getParam(iter->first, vBool))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(nh.getParam(iter->first, vInt))
else if(pnh.getParam(iter->first, vInt))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt).c_str();
}
else if(nh.getParam(iter->first, vDouble))
else if(pnh.getParam(iter->first, vDouble))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble).c_str();
@@ -2806,6 +2828,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
mapToOdomMutex_.lock();
mapToOdom_.setIdentity();
mapToOdomMutex_.unlock();
nodesToRepublish_.clear();
return true;
}
@@ -2820,8 +2843,8 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{
paused_ = true;
NODELET_INFO("rtabmap: paused!");
ros::NodeHandle nh;
nh.setParam("is_rtabmap_paused", true);
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", true);
}
return true;
}
@@ -2836,8 +2859,8 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
{
paused_ = false;
NODELET_INFO("rtabmap: resumed!");
ros::NodeHandle nh;
nh.setParam("is_rtabmap_paused", false);
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", false);
}
return true;
}
@@ -2895,6 +2918,7 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request& req,
mapToOdomMutex_.lock();
mapToOdom_.setIdentity();
mapToOdomMutex_.unlock();
nodesToRepublish_.clear();
// Open new database
databasePath_ = newDatabasePath;
@@ -3010,6 +3034,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
globalPose_.header.stamp = ros::Time(0);
gps_ = rtabmap::GPS();
tags_.clear();
nodesToRepublish_.clear();
NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
UFile::copy(databasePath_, databasePath_+".back");
@@ -3022,6 +3047,207 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
return true;
}
void CoreWrapper::republishMaps()
{
ros::Time stamp = ros::Time::now();
mapsManager_.publishMaps(rtabmap_.getLocalOptimizedPoses(), stamp, mapFrameId_);
if(mapDataPub_.getNumSubscribers())
{
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
msg->header.stamp = stamp;
msg->header.frame_id = mapFrameId_;
rtabmap_ros::mapDataToROS(
rtabmap_.getLocalOptimizedPoses(),
rtabmap_.getLocalConstraints(),
std::map<int, Signature>(),
rtabmap_.getMapCorrection(),
*msg);
mapDataPub_.publish(msg);
}
if(mapGraphPub_.getNumSubscribers())
{
rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
msg->header.stamp = stamp;
msg->header.frame_id = mapFrameId_;
rtabmap_ros::mapGraphToROS(
rtabmap_.getLocalOptimizedPoses(),
rtabmap_.getLocalConstraints(),
rtabmap_.getMapCorrection(),
*msg);
mapGraphPub_.publish(msg);
}
}
bool CoreWrapper::detectMoreLoopClosuresCallback(rtabmap_ros::DetectMoreLoopClosures::Request& req, rtabmap_ros::DetectMoreLoopClosures::Response& res)
{
NODELET_WARN("Detect more loop closures service called");
UTimer timer;
float clusterRadiusMax = 1;
float clusterRadiusMin = 0;
float clusterAngle = 0;
int iterations = 1;
bool intraSession = true;
bool interSession = true;
if(req.cluster_radius_max > 0.0f)
{
clusterRadiusMax = req.cluster_radius_max;
}
if(req.cluster_radius_min >= 0.0f)
{
clusterRadiusMin = req.cluster_radius_min;
}
if(req.cluster_angle >= 0.0f)
{
clusterAngle = req.cluster_angle;
}
if(req.iterations >= 1.0f)
{
iterations = (int)req.iterations;
}
if(req.intra_only)
{
interSession = false;
}
else if(req.inter_only)
{
intraSession = false;
}
NODELET_WARN("Post-Processing service called: Detecting more loop closures "
"(max radius=%f, min radius=%f, angle=%f, iterations=%d, intra=%s, inter=%s)...",
clusterRadiusMax,
clusterRadiusMin,
clusterAngle,
iterations,
intraSession?"true":"false",
interSession?"true":"false");
res.detected = rtabmap_.detectMoreLoopClosures(
clusterRadiusMax,
clusterAngle*M_PI/180.0,
iterations,
intraSession,
interSession,
0,
clusterRadiusMin);
if(res.detected<0)
{
NODELET_ERROR("Post-Processing: Detecting more loop closures failed!");
}
else
{
NODELET_WARN("Post-Processing: Detected %d loop closures! (%fs)", res.detected, timer.ticks());
if(res.detected>0)
{
republishMaps();
}
return true;
}
return false;
}
bool CoreWrapper::cleanupLocalGridsCallback(rtabmap_ros::CleanupLocalGrids::Request& req, rtabmap_ros::CleanupLocalGrids::Response& res)
{
NODELET_WARN("Cleanup local grids service called");
UTimer timer;
int radius = 1;
bool filterScans = false;
if(req.radius > 1.0f)
{
radius = (int)req.radius;
}
filterScans = req.filter_scans;
float xMin, yMin, gridCellSize;
cv::Mat map = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
if(map.empty())
{
NODELET_ERROR("Post-Processing: Cleanup local grids failed! There is no optimized map.");
return false;
}
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
NODELET_WARN("Post-Processing: Cleanup local grids... (radius=%d, filter scans=%s)",
radius,
filterScans?"true":"false");
res.modified = rtabmap_.cleanupLocalGrids(poses, map, xMin, yMin, gridCellSize, radius, filterScans);
if(res.modified<0)
{
NODELET_ERROR("Post-Processing: Cleanup local grids failed!");
}
else
{
if(filterScans)
{
NODELET_WARN("Post-Processing: %d grids and scans modified! (%fs)", res.modified, timer.ticks());
}
else
{
NODELET_WARN("Post-Processing: %d grids modified! (%fs)", res.modified, timer.ticks());
}
if(res.modified > 0)
{
// We should update MapsManager's cache with the modifications
mapsManager_.clear();
mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
republishMaps();
}
return true;
}
return false;
}
bool CoreWrapper::globalBundleAdjustmentCallback(rtabmap_ros::GlobalBundleAdjustment::Request& req, rtabmap_ros::GlobalBundleAdjustment::Response& res)
{
NODELET_WARN("Global bundle adjustment service called");
UTimer timer;
int optimizer = (int)Optimizer::kTypeG2O; // g2o
int iterations = Parameters::defaultOptimizerIterations();
float pixelVariance = Parameters::defaultg2oPixelVariance();
bool rematchFeatures = true;
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations);
Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance);
if(req.type == 1.0f)
{
optimizer = (int)Optimizer::kTypeCVSBA;
}
if(req.iterations >= 1.0f)
{
iterations = req.iterations;
}
if(req.pixel_variance > 0.0f)
{
pixelVariance = req.pixel_variance;
}
rematchFeatures = !req.voc_matches;
NODELET_WARN("Post-Processing: Global Bundle Adjustment... "
"(Optimizer=%s, iterations=%d, pixel variance=%f, rematch=%s)...",
optimizer==Optimizer::kTypeG2O?"g2o":"cvsba",
iterations,
pixelVariance,
rematchFeatures?"true":"false");
bool success = rtabmap_.globalBundleAdjustment((Optimizer::Type)optimizer, iterations, pixelVariance, rematchFeatures);
if(!success)
{
NODELET_ERROR("Post-Processing: Global Bundle Adjustment failed!");
}
else
{
NODELET_WARN("Post-Processing: Global Bundle Adjustment... done! (%fs)", timer.ticks());
republishMaps();
return true;
}
return false;
}
bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
NODELET_INFO("rtabmap: Set localization mode");
@@ -3836,6 +4062,58 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
{
signatures.insert(std::make_pair(stats.getLastSignatureData().id(), stats.getLastSignatureData()));
}
if(nodesToRepublish_.size() && !rtabmap_.getLastLocalizationPose().isNull())
{
// Republish data from closest nodes of the current localization
std::map<int, Transform> nodesOnly(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
if(id>0)
{
std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true);
std::map<int, int> missingIds;
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
{
missingIds.insert(std::make_pair(iter->second, iter->first));
}
}
if(nodesToRepublish_.size() != missingIds.size())
{
// remove requested nodes not anymore in the graph
for(std::set<int>::iterator iter=nodesToRepublish_.begin(); iter!=nodesToRepublish_.end();)
{
if(ids.find(*iter) == ids.end())
{
iter = nodesToRepublish_.erase(iter);
}
else
{
++iter;
}
}
}
int loaded = 0;
std::stringstream stream;
for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
{
signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
nodesToRepublish_.erase(iter->second);
++loaded;
stream << iter->second << " ";
}
if(loaded)
{
NODELET_WARN("Republishing data of requested node(s) %sfrom \"%s\" input topic (max_nodes_republished=%d)",
stream.str().c_str(),
republishNodeDataSub_.getTopic().c_str(),
maxNodesRepublished_);
}
}
}
rtabmap_ros::mapDataToROS(
stats.poses(),
stats.constraints(),
src/GuiWrapper.cpp
+12 -16
View File
@@ -72,7 +72,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
odomSensorSync_(false),
maxOdomUpdateRate_(10),
cameraNodeName_(""),
lastOdomInfoUpdateTime_(0)
lastOdomInfoUpdateTime_(0),
rtabmapNodeName_("rtabmap")
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
@@ -93,22 +94,24 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
configFile.replace('~', QDir::homePath());
pnh.param("rtabmap", rtabmapNodeName_, rtabmapNodeName_);
ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
uSleep(500);
prefDialog_ = new PreferencesDialogROS(configFile);
prefDialog_ = new PreferencesDialogROS(configFile, rtabmapNodeName_);
mainWindow_ = new MainWindow(prefDialog_);
mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
mainWindow_->show();
bool paused = false;
nh.param("is_rtabmap_paused", paused, paused);
ros::NodeHandle rnh(rtabmapNodeName_);
rnh.param("is_rtabmap_paused", paused, paused);
mainWindow_->setMonitoringState(paused);
// To receive odometry events
std::string tfPrefix;
std::string initCachePath;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
@@ -142,16 +145,9 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
QMetaObject::invokeMethod(mainWindow_, "updateCacheFromDatabase", Q_ARG(QString, QString(initCachePath.c_str())));
}
if(!tfPrefix.empty())
if(pnh.hasParam("tf_prefix"))
{
if(!frameId_.empty())
{
frameId_ = tfPrefix + "/" + frameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix + "/" + odomFrameId_;
}
ROS_ERROR("tf_prefix parameter has been removed, use directly odom_frame_id and frame_id parameters.");
}
UEventsManager::addHandler(this);
@@ -258,13 +254,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
bool modified = false;
ros::NodeHandle nh;
ros::NodeHandle rnh(rtabmapNodeName_);
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
//save only parameters with valid names
if(defaultParameters.find((*i).first) != defaultParameters.end())
{
nh.setParam((*i).first, (*i).second);
rnh.setParam((*i).first, (*i).second);
modified = true;
}
else if((*i).first.find('/') != (*i).first.npos)
src/MapsManager.cpp
+2 -5
View File
@@ -69,7 +69,7 @@ MapsManager::MapsManager() :
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
occupancyGrid_(new OccupancyGrid),
gridUpdated_(true),
octomap_(0),
octomap_(new OctoMap),
octomapTreeDepth_(16),
octomapUpdated_(true),
latching_(true)
@@ -131,10 +131,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomap_ = new OctoMap(occupancyGrid_->getCellSize(), 0.5, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
pnh.param("octomap_frontier_flood_fill", octomap_frontier_flood_fill_, false);
if(octomapTreeDepth_ > 16)
{
ROS_WARN("octomap_tree_depth maximum is 16");
@@ -1241,7 +1238,7 @@ void MapsManager::publishMaps(
pcl::IndicesPtr frontierIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0,octomap_frontier_flood_fill_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0);
if(octoMapCloud_.getNumSubscribers())
{
src/MsgConversion.cpp
+93 -153
View File
@@ -163,38 +163,43 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
{
if(!image->rgb.data.empty())
toCvShare(*image, image, rgb, depth);
}
void toCvShare(const rtabmap_ros::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
{
if(!image.rgb.data.empty())
{
rgb = cv_bridge::toCvShare(image->rgb, image);
rgb = cv_bridge::toCvShare(image.rgb, trackedObject);
}
else if(!image->rgb_compressed.data.empty())
else if(!image.rgb_compressed.data.empty())
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
rgb = cv_bridge::toCvCopy(image->rgb_compressed);
rgb = cv_bridge::toCvCopy(image.rgb_compressed);
#endif
}
if(!image->depth.data.empty())
if(!image.depth.data.empty())
{
depth = cv_bridge::toCvShare(image->depth, image);
depth = cv_bridge::toCvShare(image.depth, trackedObject);
}
else if(!image->depth_compressed.data.empty())
else if(!image.depth_compressed.data.empty())
{
if(image->depth_compressed.format.compare("jpg")==0)
if(image.depth_compressed.format.compare("jpg")==0)
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
depth = cv_bridge::toCvCopy(image->depth_compressed);
depth = cv_bridge::toCvCopy(image.depth_compressed);
#endif
}
else
{
cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
ptr->header = image->depth_compressed.header;
ptr->image = rtabmap::uncompressImage(image->depth_compressed.data);
ptr->header = image.depth_compressed.header;
ptr->image = rtabmap::uncompressImage(image.depth_compressed.data);
ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
depth = ptr;
@@ -369,7 +374,6 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
int depthHeight = depthMsg->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());
@@ -804,6 +808,25 @@ rtabmap::CameraModel cameraModelFromROS(
D.at<double>(0,4) = camInfo.D[2];
D.at<double>(0,5) = camInfo.D[3];
}
else if(camInfo.D.size()>8)
{
bool zerosAfter8 = true;
for(size_t i=8; i<camInfo.D.size() && zerosAfter8; ++i)
{
if(camInfo.D[i] != 0.0)
{
zerosAfter8 = false;
}
}
static bool warned = false;
if(!zerosAfter8 && !warned)
{
ROS_WARN("Camera info conversion: Distortion model is larger than 8, coefficients after 8 are ignored. This message is only shown once.");
warned = true;
}
D = cv::Mat(1, 8, CV_64FC1);
memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double));
}
else
{
D = cv::Mat(1, camInfo.D.size(), CV_64FC1);
@@ -1454,12 +1477,14 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ig
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
info.localScanMap = rtabmap::LaserScan(rtabmap::uncompressData(msg.localScanMap), 0, 0, (rtabmap::LaserScan::Format)msg.localScanMapFormat);
pcl::PCLPointCloud2 cloud;
pcl_conversions::toPCL(msg.localScanMap, cloud);
info.localScanMap = rtabmap::util3d::laserScanFromPointCloud(cloud);
}
return info;
}
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg, bool ignoreData)
{
msg.lost = info.lost;
msg.matches = info.reg.matches;
@@ -1493,6 +1518,13 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.type = info.type;
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
transformToGeometryMsg(info.transformGroundTruth, msg.transformGroundTruth);
transformToGeometryMsg(info.guess, msg.guess);
if(!ignoreData)
{
msg.wordsKeys = uKeys(info.words);
keypointsToROS(uValues(info.words), msg.wordsValues);
@@ -1503,16 +1535,11 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
points2fToROS(info.newCorners, msg.newCorners);
msg.cornerInliers = info.cornerInliers;
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
transformToGeometryMsg(info.transformGroundTruth, msg.transformGroundTruth);
transformToGeometryMsg(info.guess, msg.guess);
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
msg.localScanMap = rtabmap::compressData(rtabmap::util3d::transformLaserScan(info.localScanMap, info.localScanMap.localTransform()).data());
msg.localScanMapFormat = info.localScanMap.format();
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(info.localScanMap, info.localScanMap.localTransform()), msg.localScanMap);
}
}
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
@@ -1562,7 +1589,7 @@ void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool c
}
rtabmap::Landmarks landmarksFromROS(
const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
const std::string & frameId,
const std::string & odomFrameId,
const ros::Time & odomStamp,
@@ -1573,7 +1600,7 @@ rtabmap::Landmarks landmarksFromROS(
{
//tag detections
rtabmap::Landmarks landmarks;
for(std::map<int, geometry_msgs::PoseWithCovarianceStamped>::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
for(std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> >::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
{
if(iter->first <=0)
{
@@ -1582,19 +1609,19 @@ rtabmap::Landmarks landmarksFromROS(
}
rtabmap::Transform baseToCamera = rtabmap_ros::getTransform(
frameId,
iter->second.header.frame_id,
iter->second.header.stamp,
iter->second.first.header.frame_id,
iter->second.first.header.stamp,
listener,
waitForTransform);
if(baseToCamera.isNull())
{
ROS_ERROR("Cannot transform tag pose from \"%s\" frame to \"%s\" frame!",
iter->second.header.frame_id.c_str(), frameId.c_str());
iter->second.first.header.frame_id.c_str(), frameId.c_str());
continue;
}
rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.pose.pose);
rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.first.pose.pose);
if(!baseToTag.isNull())
{
@@ -1602,7 +1629,7 @@ rtabmap::Landmarks landmarksFromROS(
rtabmap::Transform correction = rtabmap_ros::getTransform(
frameId,
odomFrameId,
iter->second.header.stamp,
iter->second.first.header.stamp,
odomStamp,
listener,
waitForTransform);
@@ -1616,14 +1643,14 @@ rtabmap::Landmarks landmarksFromROS(
"If odometry is small since it received the tag pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.pose.covariance.data()).clone();
cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.first.pose.covariance.data()).clone();
if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{
covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= defaultAngVariance;
}
landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, baseToTag, covariance)));
landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, iter->second.second, baseToTag, covariance)));
}
}
return landmarks;
@@ -1719,25 +1746,26 @@ bool convertRGBDMsgs(
std::vector<cv::Point3f> * localPoints3d,
cv::Mat * localDescriptors)
{
UASSERT(imageMsgs.size()>0 &&
(imageMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
imageMsgs.size() == cameraInfoMsgs.size());
UASSERT(!cameraInfoMsgs.empty()>0 &&
(cameraInfoMsgs.size() == imageMsgs.size() || imageMsgs.empty()) &&
(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()));
int imageWidth = imageMsgs[0]->image.cols;
int imageHeight = imageMsgs[0]->image.rows;
int imageWidth = imageMsgs.size()?imageMsgs[0]->image.cols:cameraInfoMsgs[0].width;
int imageHeight = imageMsgs.size()?imageMsgs[0]->image.rows:cameraInfoMsgs[0].height;
int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;
if(depthMsgs.size())
if(!depthMsgs.empty())
{
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 = imageMsgs.size();
for(unsigned int i=0; i<imageMsgs.size(); ++i)
int cameraCount = cameraInfoMsgs.size();
for(unsigned int i=0; i<cameraInfoMsgs.size(); ++i)
{
if(!imageMsgs.empty())
{
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
@@ -1754,7 +1782,15 @@ bool convertRGBDMsgs(
imageMsgs[i]->encoding.c_str());
return false;
}
if(depthMsgs.size() &&
UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->image.rows).c_str());
}
if(!depthMsgs.empty() &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
@@ -1764,14 +1800,9 @@ bool convertRGBDMsgs(
return false;
}
UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->image.rows).c_str());
ros::Time stamp;
if(depthMsgs.size())
if(!depthMsgs.empty())
{
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
@@ -1781,13 +1812,17 @@ bool convertRGBDMsgs(
depthMsgs[i]->image.rows).c_str());
stamp = depthMsgs[i]->header.stamp;
}
else
else if(!imageMsgs.empty())
{
stamp = imageMsgs[i]->header.stamp;
}
else
{
stamp = cameraInfoMsgs[i].header.stamp;
}
// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, stamp, listener, waitForTransform);
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform);
if(localTransform.isNull())
{
ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec());
@@ -1815,6 +1850,8 @@ bool convertRGBDMsgs(
}
}
if(!imageMsgs.empty())
{
cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
@@ -1845,8 +1882,9 @@ bool convertRGBDMsgs(
ROS_ERROR("Some RGB images are not the same type!");
return false;
}
}
if(depthMsgs.size())
if(!depthMsgs.empty())
{
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
@@ -1869,16 +1907,16 @@ bool convertRGBDMsgs(
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
if(localKeyPoints && localKeyPointsMsgs.size() == imageMsgs.size())
if(localKeyPoints && localKeyPointsMsgs.size() == cameraInfoMsgs.size())
{
rtabmap_ros::keypointsFromROS(localKeyPointsMsgs[i], *localKeyPoints, imageWidth*i);
}
if(localPoints3d && localPoints3dMsgs.size() == imageMsgs.size())
if(localPoints3d && localPoints3dMsgs.size() == cameraInfoMsgs.size())
{
// Points should be in base frame
rtabmap_ros::points3fFromROS(localPoints3dMsgs[i], *localPoints3d, localTransform);
}
if(localDescriptors && localDescriptorsMsgs.size() == imageMsgs.size())
if(localDescriptors && localDescriptorsMsgs.size() == cameraInfoMsgs.size())
{
localDescriptors->push_back(localDescriptorsMsgs[i]);
}
@@ -2197,39 +2235,6 @@ bool convertScan3dMsg(
UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height,
uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str());
bool hasNormals = false;
bool hasColors = false;
bool hasIntensity = false;
for(unsigned int i=0; i<scan3dMsg.fields.size(); ++i)
{
if(scan3dMsg.fields[i].name.compare("normal_x") == 0)
{
hasNormals = true;
}
if(scan3dMsg.fields[i].name.compare("rgb") == 0 || scan3dMsg.fields[i].name.compare("rgba") == 0)
{
hasColors = true;
}
if(scan3dMsg.fields[i].name.compare("intensity") == 0)
{
if(scan3dMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
hasIntensity = true;
}
else
{
static bool warningShown = false;
if(!warningShown)
{
ROS_WARN("The input scan cloud has an \"intensity\" field "
"but the datatype (%d) is not supported. Intensity will be ignored. "
"This message is only shown once.", scan3dMsg.fields[i].datatype);
warningShown = true;
}
}
}
}
rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform);
if(scanLocalTransform.isNull())
{
@@ -2257,73 +2262,8 @@ bool convertScan3dMsg(
scanLocalTransform = sensorT * scanLocalTransform;
}
}
if(hasNormals)
{
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
else if(hasIntensity)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
}
else
{
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
else if(hasIntensity)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scan3dMsg, *pclScan);
if(!pclScan->is_dense)
{
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
}
}
scan = rtabmap::util3d::laserScanFromPointCloud(scan3dMsg);
scan = rtabmap::LaserScan(scan, maxPoints, maxRange, scanLocalTransform);
return true;
}
src/OdometryROS.cpp
+13 -10
View File
@@ -96,14 +96,6 @@ OdometryROS::~OdometryROS()
warningThread_->join();
delete warningThread_;
}
ros::NodeHandle & pnh = getPrivateNodeHandle();
if(pnh.ok())
{
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
}
delete odometry_;
}
@@ -332,6 +324,12 @@ void OdometryROS::onInit()
}
}
// set private parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.setParam(iter->first, iter->second);
}
Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
@@ -861,11 +859,15 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
if(odomInfoPub_.getNumSubscribers() || odomInfoLitePub_.getNumSubscribers())
{
rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg);
odomInfoToROS(info, infoMsg, odomInfoPub_.getNumSubscribers()==0);
infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_;
if(odomInfoPub_.getNumSubscribers()>0) {
odomInfoPub_.publish(infoMsg);
}
if(odomInfoLitePub_.getNumSubscribers()>0)
{
infoMsg.wordInliers.clear();
infoMsg.wordMatches.clear();
infoMsg.wordsKeys.clear();
@@ -875,9 +877,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
infoMsg.cornerInliers.clear();
infoMsg.localMapKeys.clear();
infoMsg.localMapValues.clear();
infoMsg.localScanMap.clear();
infoMsg.localScanMap = sensor_msgs::PointCloud2();
odomInfoLitePub_.publish(infoMsg);
}
}
if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
{
src/PreferencesDialogROS.cpp
+6 -5
View File
@@ -41,8 +41,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
configFile_(configFile)
PreferencesDialogROS::PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName) :
configFile_(configFile),
rtabmapNodeName_(rtabmapNodeName)
{
}
@@ -84,7 +85,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
path = filePath;
}
ros::NodeHandle nh;
ros::NodeHandle rnh(rtabmapNodeName_);
ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str());
bool validParameters = true;
int readCount = 0;
@@ -108,7 +109,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
double stamp = UTimer::now();
std::string tmp;
bool warned = false;
while(!nh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
while(!rnh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
{
if(!warned)
{
@@ -146,7 +147,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
else
{
std::string value;
if(nh.getParam(i->first,value))
if(rnh.getParam(i->first,value))
{
//backward compatibility
if(i->first.compare(Parameters::kIcpStrategy()) == 0)
src/RGBDXSyncNode.cpp
+47
View File
@@ -0,0 +1,47 @@
/*
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 "nodelet/loader.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "rgbdx_sync");
nodelet::V_string nargv;
for(int i=1;i<argc;++i)
{
nargv.push_back(argv[i]);
}
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "rtabmap_ros/rgbdx_sync", remap, nargv);
ros::spin();
return 0;
}
src/impl/CommonDataSubscriberDepth.cpp
+32 -32
View File
@@ -498,11 +498,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -513,11 +513,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -528,22 +528,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else
@@ -560,11 +560,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -575,11 +575,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -590,22 +590,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -622,11 +622,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -638,11 +638,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -653,22 +653,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#endif
@@ -682,11 +682,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -697,11 +697,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -712,22 +712,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
}
src/impl/CommonDataSubscriberOdom.cpp
+3 -3
View File
@@ -89,11 +89,11 @@ void CommonDataSubscriber::setupOdomCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_);
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_);
}
}
else
@@ -102,7 +102,7 @@ void CommonDataSubscriber::setupOdomCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
}
}
else
src/impl/CommonDataSubscriberRGB.cpp
+32 -32
View File
@@ -492,11 +492,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -507,11 +507,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -522,22 +522,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
}
}
else
@@ -554,11 +554,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -569,11 +569,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -584,22 +584,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -616,11 +616,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -632,11 +632,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -647,22 +647,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
}
}
#endif
@@ -676,11 +676,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
@@ -691,11 +691,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
@@ -706,22 +706,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
}
}
}
src/impl/CommonDataSubscriberRGBD.cpp
+19 -19
View File
@@ -565,7 +565,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -576,7 +576,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -587,17 +587,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
}
}
else
@@ -614,7 +614,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -625,7 +625,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -636,17 +636,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -662,7 +662,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -673,7 +673,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -684,17 +684,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
}
}
#endif
@@ -709,7 +709,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -720,7 +720,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -731,13 +731,13 @@ void CommonDataSubscriber::setupRGBDCallbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
src/impl/CommonDataSubscriberRGBD2.cpp
+20 -20
View File
@@ -374,7 +374,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -385,7 +385,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -396,17 +396,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else
@@ -423,7 +423,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -434,7 +434,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -445,17 +445,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -471,7 +471,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -482,7 +482,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -493,17 +493,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#endif
@@ -518,7 +518,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -529,7 +529,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -540,17 +540,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
}
src/impl/CommonDataSubscriberRGBD3.cpp
+20 -20
View File
@@ -461,7 +461,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -472,7 +472,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -483,17 +483,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
else
@@ -510,7 +510,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -521,7 +521,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -532,17 +532,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -558,7 +558,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); }
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); }
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
@@ -568,7 +568,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -579,17 +579,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#endif
@@ -604,7 +604,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -615,7 +615,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -626,17 +626,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
}
src/impl/CommonDataSubscriberRGBD4.cpp
+20 -20
View File
@@ -429,7 +429,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -440,7 +440,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -451,17 +451,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else
@@ -478,7 +478,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -489,7 +489,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -500,17 +500,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -526,7 +526,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -537,7 +537,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -548,17 +548,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#endif
@@ -573,7 +573,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -584,7 +584,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -595,17 +595,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
}
src/impl/CommonDataSubscriberRGBD5.cpp
+10 -10
View File
@@ -282,7 +282,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -293,7 +293,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -304,17 +304,17 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
else
@@ -328,7 +328,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -339,7 +339,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -350,17 +350,17 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL6(rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
}
src/impl/CommonDataSubscriberRGBD6.cpp
+10 -10
View File
@@ -299,7 +299,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL8(rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -310,7 +310,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL8(rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -321,17 +321,17 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL8(rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL8(rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL7(rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
else
@@ -345,7 +345,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
@@ -356,7 +356,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
@@ -367,17 +367,17 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL7(rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL7(rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
}
src/impl/CommonDataSubscriberRGBDX.cpp
+535
View File
@@ -0,0 +1,535 @@
/*
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/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_ros/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_ros {
#define IMAGE_CONVERSION() \
UASSERT(!imagesMsg->rgbd_images.empty()); \
callbackCalled(); \
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
std::vector<cv::Mat> localDescriptors; \
for(size_t i=0; i<imageMsgs.size(); ++i) \
{ \
rtabmap_ros::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
if(!imagesMsg->rgbd_images[i].global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(imagesMsg->rgbd_images[i].global_descriptor); \
localKeyPoints.push_back(imagesMsg->rgbd_images[i].key_points); \
localPoints3d.push_back(imagesMsg->rgbd_images[i].points); \
localDescriptors.push_back(rtabmap::uncompressData(imagesMsg->rgbd_images[i].descriptors)); \
} \
if(!depthMsgs[0].get()) \
depthMsgs.clear();
// X RGBD
void CommonDataSubscriber::rgbdXCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan2dCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan3dCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScanDescCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXInfoCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom
void CommonDataSubscriber::rgbdXOdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
// X RGBD + User Data
void CommonDataSubscriber::rgbdXDataCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScanDescCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataInfoCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom + User Data
void CommonDataSubscriber::rgbdXOdomDataCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
void CommonDataSubscriber::setupRGBDXCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup rgbdX callback");
rgbdXSub_.subscribe(nh, "rgbd_images", queueSize);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(nh, "odom", queueSize);
userDataSub_.subscribe(nh, "user_data", queueSize);
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", queueSize);
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(nh, "user_data", queueSize);
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_);
}
}
#endif
else
{
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
}
else
{
rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this);
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
rgbdXSubOnly_.getTopic().c_str());
}
}
}
} /* namespace rtabmap_ros */
src/impl/CommonDataSubscriberScan.cpp
+20 -20
View File
@@ -310,11 +310,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -323,11 +323,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
}
}
else
@@ -336,11 +336,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL4(odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
}
}
}
@@ -356,11 +356,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -369,11 +369,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
}
}
else
@@ -382,11 +382,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
}
}
}
@@ -401,11 +401,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -418,7 +418,7 @@ void CommonDataSubscriber::setupScanCallbacks(
}
else
{
SYNC_DECL2(dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
}
}
else
@@ -427,11 +427,11 @@ void CommonDataSubscriber::setupScanCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
}
}
}
@@ -442,15 +442,15 @@ void CommonDataSubscriber::setupScanCallbacks(
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
if(scanDescTopic)
{
SYNC_DECL2(scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
}
else if(scan2dTopic)
{
SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
}
}
}
src/impl/CommonDataSubscriberStereo.cpp
+4 -4
View File
@@ -121,11 +121,11 @@ void CommonDataSubscriber::setupStereoCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
else
@@ -134,11 +134,11 @@ void CommonDataSubscriber::setupStereoCallbacks(
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
}
src/nodelets/icp_odometry.cpp
+6 -3
View File
@@ -86,6 +86,8 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 1;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
@@ -131,6 +133,7 @@ private:
pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
}
NODELET_INFO("IcpOdometry: queue_size = %d", queueSize);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_range_min = %f m", scanRangeMin_);
@@ -140,8 +143,8 @@ private:
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
}
@@ -535,7 +538,7 @@ private:
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
}
else if(cloudMsg.height > 1 && scanCloudMaxPoints_ != cloudMsg.height * cloudMsg.width)
else if(cloudMsg.height > 1 && scanCloudMaxPoints_ < cloudMsg.height * cloudMsg.width)
{
NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
src/nodelets/point_cloud_aggregator.cpp
+1 -1
View File
@@ -293,7 +293,7 @@ private:
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
#endif
//Make sure row_step is the sum of both
tmp_output->row_step = output->row_step + cloud2->row_step;
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
}
src/nodelets/point_cloud_assembler.cpp
+147 -5
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/exact_time.h>
@@ -50,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/OdomInfo.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/Version.h>
namespace rtabmap_ros
{
@@ -75,12 +77,15 @@ public:
skipClouds_(0),
cloudsSkipped_(0),
circularBuffer_(false),
linearUpdate_(0),
angularUpdate_(0),
waitForTransformDuration_(0.1),
rangeMin_(0),
rangeMax_(0),
voxelSize_(0),
noiseRadius_(0),
noiseMinNeighbors_(5),
removeZ_(false),
fixedFrameId_("odom"),
frameId_("")
{}
@@ -114,14 +119,16 @@ private:
pnh.param("assembling_time", assemblingTime_, assemblingTime_);
pnh.param("skip_clouds", skipClouds_, skipClouds_);
pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
pnh.param("linear_update", linearUpdate_, linearUpdate_);
pnh.param("angular_update", angularUpdate_, angularUpdate_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("range_min", rangeMin_, rangeMin_);
pnh.param("range_max", rangeMax_, rangeMax_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("noise_radius", noiseRadius_, noiseRadius_);
pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
pnh.param("remove_z", removeZ_, removeZ_);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
@@ -130,12 +137,21 @@ private:
ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false");
ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_);
ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_);
ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_);
ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false");
if(maxClouds_==0 && assemblingTime_ ==0.0)
{
ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
exit(-1);
}
cloudsSkipped_ = skipClouds_;
@@ -225,6 +241,50 @@ private:
}
}
sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field)
{
sensor_msgs::PointCloud2 output;
int offset = 0;
std::vector<int> inputFieldIndex;
for(size_t i=0; i<input.fields.size(); ++i)
{
if(input.fields[i].name.compare(field) == 0)
{
continue;
}
else
{
sensor_msgs::PointField outputField = input.fields[i];
outputField.offset = offset;
offset += outputField.count * sizeOfPointField(outputField.datatype);
output.fields.push_back(outputField);
inputFieldIndex.push_back(i);
}
}
output.header = input.header;
output.height = input.height;
output.width = input.width;
output.is_bigendian = input.is_bigendian;
output.is_dense = input.is_dense;
output.point_step = offset;
output.row_step = output.width * output.point_step;
output.data.resize(output.height*output.row_step);
int total = output.height*output.width;
for(int i=0; i<total; ++i)
{
// for each point, copy fields
int oi = i*output.point_step;
int pi = i*input.point_step;
for(size_t j=0;j<output.fields.size(); ++j)
{
memcpy(&output.data[oi + output.fields[j].offset],
&input.data[pi + input.fields[inputFieldIndex[j]].offset],
output.fields[j].count * sizeOfPointField(output.fields[j].datatype));
}
}
return output;
}
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
if(cloudPub_.getNumSubscribers())
@@ -236,20 +296,35 @@ private:
{
cloudsSkipped_ = 0;
rtabmap::Transform t = rtabmap_ros::getTransform(
rtabmap::Transform pose = rtabmap_ros::getTransform(
fixedFrameId_, //fromFrame
cloudMsg->header.frame_id, //toFrame
cloudMsg->header.stamp,
tfListener_,
waitForTransformDuration_);
if(t.isNull())
if(pose.isNull())
{
ROS_ERROR("Cloud not transform all clouds! Resetting...");
clouds_.clear();
return;
}
bool isMoving = true;
if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
{
rtabmap::Transform delta = previousPose_.inverse()*pose;
float roll, pitch, yaw;
delta.getEulerAngles(roll, pitch, yaw);
isMoving = fabs(delta.x()) > linearUpdate_ ||
fabs(delta.y()) > linearUpdate_ ||
fabs(delta.z()) > linearUpdate_ ||
(angularUpdate_>0.0f && (
fabs(roll) > angularUpdate_ ||
fabs(pitch) > angularUpdate_ ||
fabs(yaw) > angularUpdate_));
}
pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
{
@@ -261,13 +336,13 @@ private:
#else
pcl::uint64_t stamp = newCloud->header.stamp;
#endif
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, t);
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
newCloud->header.stamp = stamp;
}
else
{
sensor_msgs::PointCloud2 output;
pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsg, output);
pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
pcl_conversions::toPCL(output, *newCloud);
}
@@ -316,6 +391,45 @@ private:
sensor_msgs::PointCloud2 rosCloud;
if(voxelSize_>0.0)
{
// estimate if there would be an overflow
int x_idx=-1, y_idx=-1, z_idx=-1;
for (std::size_t d = 0; d < assembled->fields.size (); ++d)
{
if (assembled->fields[d].name.compare("x")==0)
x_idx = d;
if (assembled->fields[d].name.compare("y")==0)
y_idx = d;
if (assembled->fields[d].name.compare("z")==0)
z_idx = d;
}
bool overflow = false;
if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
Eigen::Vector4f min_p, max_p;
pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
float inverseVoxelSize = 1.0f/voxelSize_;
std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
{
overflow = true;
}
}
if(overflow)
{
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
std::uint64_t stamp = assembled->header.stamp;
#else
pcl::uint64_t stamp = assembled->header.stamp;
#endif
assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
assembled->header.stamp = stamp;
}
else
{
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
@@ -324,6 +438,7 @@ private:
filter.filter(*output);
assembled = output;
}
}
if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
{
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
@@ -336,6 +451,7 @@ private:
}
pcl_conversions::moveFromPCL(*assembled, rosCloud);
rtabmap::Transform t = pose;
if(!frameId_.empty())
{
// transform in target frame_id instead of sensor frame
@@ -354,6 +470,11 @@ private:
}
pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
if(removeZ_)
{
rosCloud = removeField(rosCloud, "z");
}
rosCloud.header = cloudMsg->header;
if(!frameId_.empty())
{
@@ -362,16 +483,33 @@ private:
cloudPub_.publish(rosCloud);
if(circularBuffer_)
{
if(!isMoving)
{
clouds_.pop_back();
}
else
{
previousPose_ = pose;
if(reachedMaxSize)
{
clouds_.pop_front();
}
}
}
else
{
clouds_.clear();
previousPose_.setNull();
}
}
else if(!isMoving)
{
clouds_.pop_back();
}
else
{
previousPose_ = pose;
}
}
else
{
@@ -416,6 +554,8 @@ private:
int skipClouds_;
int cloudsSkipped_;
bool circularBuffer_;
double linearUpdate_;
double angularUpdate_;
double assemblingTime_;
double waitForTransformDuration_;
double rangeMin_;
@@ -423,9 +563,11 @@ private:
double voxelSize_;
double noiseRadius_;
int noiseMinNeighbors_;
bool removeZ_;
std::string fixedFrameId_;
std::string frameId_;
tf::TransformListener tfListener_;
rtabmap::Transform previousPose_;
std::list<pcl::PCLPointCloud2::Ptr> clouds_;
};
src/nodelets/rgbd_odometry.cpp
+99 -3
View File
@@ -69,6 +69,8 @@ public:
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
queueSize_(5),
keepColor_(false)
{
@@ -133,9 +135,9 @@ private:
{
rgbdCameras = 1;
}
if(rgbdCameras > 4)
if(rgbdCameras > 5)
{
NODELET_FATAL("Only 4 cameras maximum supported yet.");
NODELET_FATAL("Only 5 cameras maximum supported yet.");
}
pnh.param("keep_color", keepColor_, keepColor_);
@@ -160,6 +162,10 @@ private:
{
rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1);
}
if(rgbdCameras == 2)
{
@@ -242,6 +248,40 @@ private:
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str());
}
else if(rgbdCameras == 5)
{
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\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(),
rgbd_image5_sub_.getTopic().c_str());
}
}
else
{
@@ -331,7 +371,6 @@ private:
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());
@@ -554,6 +593,34 @@ private:
}
}
void callbackRGBD5(
const rtabmap_ros::RGBDImageConstPtr& image,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4,
const rtabmap_ros::RGBDImageConstPtr& image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
std::vector<sensor_msgs::CameraInfo> 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]);
rtabmap_ros::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
infoMsgs.push_back(image->rgb_camera_info);
infoMsgs.push_back(image2->rgb_camera_info);
infoMsgs.push_back(image3->rgb_camera_info);
infoMsgs.push_back(image4->rgb_camera_info);
infoMsgs.push_back(image5->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
protected:
virtual void flushCallbacks()
{
@@ -630,6 +697,30 @@ protected:
rgbd_image4_sub_);
exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
}
if(approxSync5_)
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
if(exactSync5_)
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
}
private:
@@ -642,6 +733,7 @@ private:
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image5_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -659,6 +751,10 @@ private:
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync5Policy;
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
int queueSize_;
bool keepColor_;
};
src/nodelets/rgbdx_sync.cpp
+307
View File
@@ -0,0 +1,307 @@
/*
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 <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <boost/thread.hpp>
#include "rtabmap_ros/RGBDImages.h"
#include "rtabmap_ros/CommonDataSubscriber.h"
namespace rtabmap_ros
{
class RGBDXSync : public nodelet::Nodelet
{
public:
RGBDXSync() :
warningThread_(0),
callbackCalled_(false),
SYNC_INIT(rgbd2),
SYNC_INIT(rgbd3),
SYNC_INIT(rgbd4),
SYNC_INIT(rgbd5),
SYNC_INIT(rgbd6),
SYNC_INIT(rgbd7),
SYNC_INIT(rgbd8)
{}
virtual ~RGBDXSync()
{
SYNC_DEL(rgbd2);
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;
int rgbdCameras = 2;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
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: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
rgbdImagesPub_ = nh.advertise<rtabmap_ros::RGBDImages>("rgbd_images", 1);
ROS_ASSERT(rgbdCameras>=2 && rgbdCameras<=8);
rgbdSubs_.resize(rgbdCameras);
for(int i=0; i<rgbdCameras; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
}
std::string name_ = this->getName();
std::string subscribedTopicsMsg_;
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
else if(rgbdCameras==4)
{
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
else if(rgbdCameras==5)
{
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
else if(rgbdCameras==6)
{
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
else if(rgbdCameras==7)
{
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
}
else if(rgbdCameras==8)
{
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
}
warningThread_ = new boost::thread(boost::bind(&RGBDXSync::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());
}
}
}
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS5(rgbd5, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS6(rgbd6, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS7(rgbd7, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS8(rgbd8, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
private:
boost::thread * warningThread_;
bool callbackCalled_;
ros::Publisher rgbdImagesPub_;
std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
};
void RGBDXSync::rgbd2Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd3Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd4Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd5Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
output.rgbd_images[4]=(*image4);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd6Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4,
const rtabmap_ros::RGBDImageConstPtr& image5)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
output.rgbd_images[4]=(*image4);
output.rgbd_images[5]=(*image5);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd7Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4,
const rtabmap_ros::RGBDImageConstPtr& image5,
const rtabmap_ros::RGBDImageConstPtr& image6)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
output.rgbd_images[4]=(*image4);
output.rgbd_images[5]=(*image5);
output.rgbd_images[6]=(*image6);
rgbdImagesPub_.publish(output);
}
void RGBDXSync::rgbd8Callback(
const rtabmap_ros::RGBDImageConstPtr& image0,
const rtabmap_ros::RGBDImageConstPtr& image1,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4,
const rtabmap_ros::RGBDImageConstPtr& image5,
const rtabmap_ros::RGBDImageConstPtr& image6,
const rtabmap_ros::RGBDImageConstPtr& image7)
{
callbackCalled_ = true;
rtabmap_ros::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(8);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
output.rgbd_images[4]=(*image4);
output.rgbd_images[5]=(*image5);
output.rgbd_images[6]=(*image6);
output.rgbd_images[7]=(*image7);
rgbdImagesPub_.publish(output);
}
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDXSync, nodelet::Nodelet);
}
src/rviz/MapCloudDisplay.cpp
+66 -58
View File
@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Graph.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap_ros/GetMap.h>
#include <std_msgs/Int32MultiArray.h>
namespace rtabmap_ros
@@ -90,7 +91,8 @@ MapCloudDisplay::MapCloudDisplay()
new_xyz_transformer_(false),
new_color_transformer_(false),
needs_retransform_(false),
transformer_class_loader_(NULL)
transformer_class_loader_(NULL),
current_map_updated_(false)
{
//QIcon icon;
//this->setIcon(icon);
@@ -186,6 +188,8 @@ MapCloudDisplay::MapCloudDisplay()
node_filtering_angle_->setMin( 0.0f );
node_filtering_angle_->setMax( 359.0f );
download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
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 );
@@ -194,6 +198,8 @@ MapCloudDisplay::MapCloudDisplay()
"Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
this, SLOT( downloadGraph() ), this );
downloadNamespaceChanged();
// PointCloudCommon sets up a callback queue with a thread for each
// instance. Use that for processing incoming messages.
update_nh_.setCallbackQueue( &cbqueue_ );
@@ -385,6 +391,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
{
boost::mutex::scoped_lock lock(current_map_mutex_);
current_map_ = poses;
current_map_updated_ = true;
}
}
@@ -527,18 +534,17 @@ void MapCloudDisplay::updateCloudParameters()
// do nothing... only take effect on next generated clouds
}
void MapCloudDisplay::downloadMap()
void MapCloudDisplay::downloadMap(bool graphOnly)
{
if(download_map_->getBool())
{
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = true;
getMapSrv.request.global = false;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = false;
ros::NodeHandle nh;
getMapSrv.request.graphOnly = graphOnly;
std::string rtabmapNs = download_namespace->getStdString();
std::string srvName = update_nh_.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
QMessageBox * messageBox = new QMessageBox(
QMessageBox::NoIcon,
tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
tr("Calling \"%1\" service...").arg(srvName.c_str()),
tr("Downloading the map... please wait (rviz could become gray!)"),
QMessageBox::NoButton);
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
@@ -546,18 +552,26 @@ void MapCloudDisplay::downloadMap()
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))
if(!ros::service::call(srvName, 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: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
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: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
arg(nh.resolveName("rtabmap/get_map_data").c_str()));
ROS_ERROR("MapCloudDisplay: Cannot call \"%s\" service. "
"Tip: if rtabmap node is not in \"%s\" namespace, you can "
"change the \"Download namespace\" option.",
srvName.c_str(),
rtabmapNs.c_str());
messageBox->setText(tr("MapCloudDisplay: Cannot call \"%1\" service. "
"Tip: if rtabmap node is not in \"%2\" namespace, you can "
"change the \"Download namespace\" option.").
arg(srvName.c_str()).arg(rtabmapNs.c_str()));
}
else if(graphOnly)
{
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()));
}
else
{
@@ -571,6 +585,20 @@ void MapCloudDisplay::downloadMap()
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
}
void MapCloudDisplay::downloadNamespaceChanged()
{
std::string rtabmapNs = download_namespace->getStdString();
std::string topicName = update_nh_.resolveName(uFormat("%s/republish_node_data", rtabmapNs.c_str()));
republishNodeDataPub_ = update_nh_.advertise<std_msgs::Int32MultiArray>(topicName, 1);
}
void MapCloudDisplay::downloadMap()
{
if(download_map_->getBool())
{
downloadMap(false);
download_map_->blockSignals(true);
download_map_->setBool(false);
download_map_->blockSignals(false);
@@ -589,43 +617,7 @@ 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: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
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: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
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()));
}
downloadMap(true);
download_graph_->blockSignals(true);
download_graph_->setBool(false);
download_graph_->blockSignals(false);
@@ -728,6 +720,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
boost::mutex::scoped_lock lock(current_map_mutex_);
if(!current_map_.empty())
{
std::vector<int> missingNodes;
for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
{
std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
@@ -759,9 +752,15 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
}
else
{
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
it->first,
cloudInfoIt->second->message_->header.frame_id.c_str(),
cloudInfoIt->second->message_->header.frame_id.c_str());
}
}
else if(it->first>0 && current_map_updated_)
{
missingNodes.push_back(it->first);
}
}
//hide not used clouds
@@ -786,8 +785,16 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
++iter;
}
}
if(!missingNodes.empty())
{
std_msgs::Int32MultiArray msg;
msg.data = missingNodes;
republishNodeDataPub_.publish(msg);
}
}
current_map_updated_ = false;
}
if(lastCloudAdded>0)
{
lastCloudAdded_ = lastCloudAdded;
@@ -808,6 +815,7 @@ void MapCloudDisplay::reset()
{
boost::mutex::scoped_lock lock(current_map_mutex_);
current_map_.clear();
current_map_updated_ = false;
}
MFDClass::reset();
}
src/rviz/MapCloudDisplay.h
+5
View File
@@ -119,6 +119,7 @@ public:
rviz::FloatProperty* cloud_filter_ceiling_height_;
rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_;
rviz::StringProperty * download_namespace;
rviz::BoolProperty* download_map_;
rviz::BoolProperty* download_graph_;
@@ -134,6 +135,7 @@ private Q_SLOTS:
void setXyzTransformerOptions( EnumProperty* prop );
void setColorTransformerOptions( EnumProperty* prop );
void updateCloudParameters();
void downloadNamespaceChanged();
void downloadMap();
void downloadGraph();
@@ -145,6 +147,7 @@ protected:
virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );
private:
void downloadMap(bool graphOnly);
void processMapData(const rtabmap_ros::MapData& map);
/**
@@ -165,6 +168,7 @@ private:
private:
ros::AsyncSpinner spinner_;
ros::CallbackQueue cbqueue_;
ros::Publisher republishNodeDataPub_;
std::map<int, CloudInfoPtr> cloud_infos_;
@@ -173,6 +177,7 @@ private:
std::map<int, rtabmap::Transform> current_map_;
boost::mutex current_map_mutex_;
bool current_map_updated_;
int lastCloudAdded_;
srv/CleanupLocalGrids.srv
+24
View File
@@ -0,0 +1,24 @@
# Cleanup local grids service
#
# Clear empty space from local occupancy grids
# (and laser scans) based on the current optimized global 2d grid map.
# If the map needs to be regenerated in the future (e.g., when
# we re-use the map in SLAM mode), removed obstacles won't reappear.
# Use this with care and only when you know that the map doesn't have errors,
# otherwise some real obstacles/walls may be cleared if there is too much
# drift in the map.
#
# Radius in cells around empty cell without obstacles to clear underlying obstacles, default 1 cell if not set.
int32 radius
# Filter also the scans, default false if not set.
# The filtered laser scans will be used for localization,
# so if dynamic obstacles have been removed, localization won't try to
# match them anymore. Filtering the laser scans cannot be reverted,
# but grids can (see DatabaseViewer->Edit menu).
bool filter_scans
---
# return the number of grids or scans modified, -1 if there is an error
int32 modified
srv/DetectMoreLoopClosures.srv
+27
View File
@@ -0,0 +1,27 @@
# Detect more loop closures service
#
# Based on the current optimized graph,
# this process will try to find more nodes corresponding with each
# other, and thus finding more loop closures to add to graph.
#
# Cluster radius (m), default 1 m if not set
float32 cluster_radius_max
# Cluster radius min (m), default 0 m if not set
float32 cluster_radius_min
# Cluster angle (deg), default 0 deg if not set
float32 cluster_angle
# Iterations, default 1 if not set
int32 iterations
# Add only intra session loop closures
bool intra_only
# Add only inter session loop closures
bool inter_only
---
# return the number of loop closures detected, or -1 if it failed.
int32 detected
srv/GlobalBundleAdjustment.srv
+22
View File
@@ -0,0 +1,22 @@
# Global Bundle Adjustment service
#
# Perform global bundle adjustment. Note that as soon as the map
# is modified again, the graph is re-optimized the standard way (without SBA).
# It then makes only sense to use this after a mapping run (and after a call
# to /rtabmap/pause) when you know that the robot will restart in localization
# mode the next time, or at the beginning of the localization session.
#
# Optimizer type (0=g2o, 1=CVSBA), default 0
int32 type
# Iterations, default 0 (use Optimizer/Iterations already loaded in the node)
int32 iterations
# Pixel variance, default 0 (use g2o/PixelVariance already loaded in the node)
float32 pixel_variance
# Use vocabulary matches, default false (rematch all features between frames)
bool voc_matches
---
# return false if failure