i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 20:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added RTABMAP_SYNC_USER_DATA (default OFF) and RTABMAP_SYNC_MULTI_RGBD (default ON, OFF on windows) build options. Fixed some compilation errors on Windows.

Browse Source
This commit is contained in:
matlabbe
2020-03-12 01:03:35 -04:00
parent c73ddc0577
commit aec4fe6bcc
14 changed files with 234 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+85 -23
View File
@@ -35,9 +35,32 @@ find_package(RTABMap 0.19.5 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
find_package(PCL 1.7 REQUIRED) IF(RTABMAP_GUI)
FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
ELSE()
FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
ENDIF()
add_definitions(${PCL_DEFINITIONS}) # To include -march=native if set add_definitions(${PCL_DEFINITIONS}) # To include -march=native if set
IF(WIN32)
add_compile_options(-bigobj)
ENDIF(WIN32)
IF(WIN32)
option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support" OFF)
ELSE()
option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support" ON)
ENDIF()
option(RTABMAP_SYNC_USER_DATA "Build with input user data support" OFF)
MESSAGE(STATUS "RTABMAP_SYNC_MULTI_RGBD = ${RTABMAP_SYNC_MULTI_RGBD}")
MESSAGE(STATUS "RTABMAP_SYNC_USER_DATA = ${RTABMAP_SYNC_USER_DATA}")
IF(RTABMAP_SYNC_MULTI_RGBD)
add_definitions("-DRTABMAP_SYNC_MULTI_RGBD")
ENDIF(RTABMAP_SYNC_MULTI_RGBD)
IF(RTABMAP_SYNC_USER_DATA)
add_definitions("-DRTABMAP_SYNC_USER_DATA")
ENDIF(RTABMAP_SYNC_USER_DATA)
#Qt stuff #Qt stuff
# If librtabmap_gui.so is found, rtabmapviz will be built # If librtabmap_gui.so is found, rtabmapviz will be built
# If rviz is found, plugins will be built # If rviz is found, plugins will be built
@@ -174,13 +197,18 @@ SET(rtabmap_sync_lib_src
src/impl/CommonDataSubscriberStereo.cpp src/impl/CommonDataSubscriberStereo.cpp
src/impl/CommonDataSubscriberRGB.cpp src/impl/CommonDataSubscriberRGB.cpp
src/impl/CommonDataSubscriberRGBD.cpp src/impl/CommonDataSubscriberRGBD.cpp
src/impl/CommonDataSubscriberRGBD2.cpp
src/impl/CommonDataSubscriberRGBD3.cpp
src/impl/CommonDataSubscriberRGBD4.cpp
src/impl/CommonDataSubscriberScan.cpp src/impl/CommonDataSubscriberScan.cpp
src/impl/CommonDataSubscriberOdom.cpp src/impl/CommonDataSubscriberOdom.cpp
src/CoreWrapper.cpp # we put CoreWrapper here instead of plugins lib to avoid long compilation time on plugins lib src/CoreWrapper.cpp # we put CoreWrapper here instead of plugins lib to avoid long compilation time on plugins lib
) )
IF(RTABMAP_SYNC_MULTI_RGBD)
SET(rtabmap_sync_lib_src
${rtabmap_sync_lib_src}
src/impl/CommonDataSubscriberRGBD2.cpp
src/impl/CommonDataSubscriberRGBD3.cpp
src/impl/CommonDataSubscriberRGBD4.cpp
)
ENDIF(RTABMAP_SYNC_MULTI_RGBD)
SET(rtabmap_ros_lib_src SET(rtabmap_ros_lib_src
src/MsgConversion.cpp src/MsgConversion.cpp
@@ -305,9 +333,11 @@ add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
target_link_libraries(rtabmap_imu_to_tf ${Libraries}) target_link_libraries(rtabmap_imu_to_tf ${Libraries})
set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf")
IF(NOT WIN32)
add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp) add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp)
target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros) target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros)
set_target_properties(rtabmap_wifi_signal_pub PROPERTIES OUTPUT_NAME "wifi_signal_pub") set_target_properties(rtabmap_wifi_signal_pub PROPERTIES OUTPUT_NAME "wifi_signal_pub")
ENDIF(NOT WIN32)
add_executable(rtabmap_wifi_signal_sub src/WifiSignalSubNode.cpp) add_executable(rtabmap_wifi_signal_sub src/WifiSignalSubNode.cpp)
target_link_libraries(rtabmap_wifi_signal_sub rtabmap_ros) target_link_libraries(rtabmap_wifi_signal_sub rtabmap_ros)
set_target_properties(rtabmap_wifi_signal_sub PROPERTIES OUTPUT_NAME "wifi_signal_sub") set_target_properties(rtabmap_wifi_signal_sub PROPERTIES OUTPUT_NAME "wifi_signal_sub")
@@ -377,35 +407,67 @@ IF(rviz_FOUND)
## RVIZ plugin ## RVIZ plugin
IF(QT4_FOUND) IF(QT4_FOUND)
qt4_wrap_cpp(MOC_FILES IF(WIN32)
src/rviz/MapCloudDisplay.h qt4_wrap_cpp(MOC_FILES
src/rviz/MapGraphDisplay.h src/rviz/MapCloudDisplay.h
src/rviz/InfoDisplay.h src/rviz/MapGraphDisplay.h
src/rviz/OrbitOrientedViewController.h src/rviz/InfoDisplay.h
) )
ELSE()
qt4_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ENDIF()
ELSE() ELSE()
qt5_wrap_cpp(MOC_FILES IF(WIN32)
src/rviz/MapCloudDisplay.h qt5_wrap_cpp(MOC_FILES
src/rviz/MapGraphDisplay.h src/rviz/MapCloudDisplay.h
src/rviz/InfoDisplay.h src/rviz/MapGraphDisplay.h
src/rviz/OrbitOrientedViewController.h src/rviz/InfoDisplay.h
) )
ELSE()
qt5_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ENDIF()
ENDIF() ENDIF()
# tf:message_filters, mixing boost and Qt signals # tf:message_filters, mixing boost and Qt signals
set_property( IF(WIN32)
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp set_property(
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp
) PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
add_library(rtabmap_rviz_plugins )
src/rviz/MapCloudDisplay.cpp ELSE()
set_property(
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
)
ENDIF()
SET(SRC_FILES
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES} ${MOC_FILES}
)
IF(NOT WIN32)
SET(SRC_FILES
${SRC_FILES}
src/rviz/OrbitOrientedViewController.cpp
)
ENDIF(NOT WIN32)
add_library(rtabmap_rviz_plugins
${SRC_FILES}
) )
target_link_libraries(rtabmap_rviz_plugins target_link_libraries(rtabmap_rviz_plugins
rtabmap_ros rtabmap_ros
${Libraries}
) )
IF(Qt5_FOUND) IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui) QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
include/rtabmap_ros/CommonDataSubscriber.h
+20
View File
@@ -158,6 +158,7 @@ private:
bool subscribeOdomInfo, bool subscribeOdomInfo,
int queueSize, int queueSize,
bool approxSync); bool approxSync);
#ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks( void setupRGBD2Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -188,6 +189,7 @@ private:
bool subscribeOdomInfo, bool subscribeOdomInfo,
int queueSize, int queueSize,
bool approxSync); bool approxSync);
#endif
void setupScanCallbacks( void setupScanCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -264,6 +266,7 @@ private:
DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// RGB + Depth + User Data // RGB + Depth + User Data
DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo); DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
@@ -279,6 +282,7 @@ private:
DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
// Stereo // Stereo
DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo); DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
@@ -304,6 +308,7 @@ private:
DATA_SYNCS5(rgbOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// RGB-only + User Data // RGB-only + User Data
DATA_SYNCS3(rgbData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo); DATA_SYNCS3(rgbData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS4(rgbDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan); DATA_SYNCS4(rgbDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
@@ -319,6 +324,7 @@ private:
DATA_SYNCS5(rgbOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
// 1 RGBD // 1 RGBD
void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&); void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&);
@@ -336,6 +342,7 @@ private:
DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 1 RGBD + User Data // 1 RGBD + User Data
DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage); DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
@@ -351,7 +358,9 @@ private:
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD // 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
@@ -368,6 +377,7 @@ private:
DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
@@ -383,6 +393,7 @@ private:
DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
// 3 RGBD // 3 RGBD
DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -400,6 +411,7 @@ private:
DATA_SYNCS6(rgbd3OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbd3OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbd3OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbd3OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 3 RGBD + User Data // 3 RGBD + User Data
DATA_SYNCS4(rgbd3Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS4(rgbd3Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS5(rgbd3DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS5(rgbd3DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
@@ -415,6 +427,7 @@ private:
DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS7(rgbd3OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS7(rgbd3OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS7(rgbd3OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS7(rgbd3OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
// 4 RGBD // 4 RGBD
DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
@@ -432,6 +445,7 @@ private:
DATA_SYNCS7(rgbd4OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS7(rgbd4OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS7(rgbd4OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS7(rgbd4OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 4 RGBD + User Data // 4 RGBD + User Data
DATA_SYNCS5(rgbd4Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage); DATA_SYNCS5(rgbd4Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS6(rgbd4DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan); DATA_SYNCS6(rgbd4DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
@@ -447,6 +461,8 @@ private:
DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo); DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS8(rgbd4OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS8(rgbd4OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS8(rgbd4OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS8(rgbd4OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
#endif //RTABMAP_SYNC_MULTI_RGBD
// Scan // Scan
void scan2dCallback(const sensor_msgs::LaserScanConstPtr&); void scan2dCallback(const sensor_msgs::LaserScanConstPtr&);
@@ -460,6 +476,7 @@ private:
DATA_SYNCS3(odomScan2dInfo, nav_msgs::Odometry, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS3(odomScan2dInfo, nav_msgs::Odometry, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS3(odomScan3dInfo, nav_msgs::Odometry, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS3(odomScan3dInfo, nav_msgs::Odometry, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// Scan + User Data // Scan + User Data
DATA_SYNCS2(dataScan2d, rtabmap_ros::UserData, sensor_msgs::LaserScan); DATA_SYNCS2(dataScan2d, rtabmap_ros::UserData, sensor_msgs::LaserScan);
DATA_SYNCS2(dataScan3d, rtabmap_ros::UserData, sensor_msgs::PointCloud2); DATA_SYNCS2(dataScan3d, rtabmap_ros::UserData, sensor_msgs::PointCloud2);
@@ -471,14 +488,17 @@ private:
DATA_SYNCS3(odomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2); DATA_SYNCS3(odomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2);
DATA_SYNCS4(odomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo); DATA_SYNCS4(odomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS4(odomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo); DATA_SYNCS4(odomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
#endif
// Odom // Odom
void odomCallback(const nav_msgs::OdometryConstPtr&); void odomCallback(const nav_msgs::OdometryConstPtr&);
DATA_SYNCS2(odomInfo, nav_msgs::Odometry, rtabmap_ros::OdomInfo); DATA_SYNCS2(odomInfo, nav_msgs::Odometry, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// Odom + User Data // Odom + User Data
DATA_SYNCS2(odomData, nav_msgs::Odometry, rtabmap_ros::UserData); DATA_SYNCS2(odomData, nav_msgs::Odometry, rtabmap_ros::UserData);
DATA_SYNCS3(odomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::OdomInfo); DATA_SYNCS3(odomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::OdomInfo);
#endif
}; };
} /* namespace rtabmap_ros */ } /* namespace rtabmap_ros */
src/CommonDataSubscriber.cpp
+54 -2
View File
@@ -59,6 +59,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(depthOdomScan2dInfo), SYNC_INIT(depthOdomScan2dInfo),
SYNC_INIT(depthOdomScan3dInfo), SYNC_INIT(depthOdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// RGB + Depth + User Data // RGB + Depth + User Data
SYNC_INIT(depthData), SYNC_INIT(depthData),
SYNC_INIT(depthDataScan2d), SYNC_INIT(depthDataScan2d),
@@ -74,6 +75,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(depthOdomDataInfo), SYNC_INIT(depthOdomDataInfo),
SYNC_INIT(depthOdomDataScan2dInfo), SYNC_INIT(depthOdomDataScan2dInfo),
SYNC_INIT(depthOdomDataScan3dInfo), SYNC_INIT(depthOdomDataScan3dInfo),
#endif
// Stereo // Stereo
SYNC_INIT(stereo), SYNC_INIT(stereo),
@@ -99,6 +101,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbOdomScan2dInfo), SYNC_INIT(rgbOdomScan2dInfo),
SYNC_INIT(rgbOdomScan3dInfo), SYNC_INIT(rgbOdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// RGB-only + User Data // RGB-only + User Data
SYNC_INIT(rgbData), SYNC_INIT(rgbData),
SYNC_INIT(rgbDataScan2d), SYNC_INIT(rgbDataScan2d),
@@ -114,7 +117,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbOdomDataInfo), SYNC_INIT(rgbOdomDataInfo),
SYNC_INIT(rgbOdomDataScan2dInfo), SYNC_INIT(rgbOdomDataScan2dInfo),
SYNC_INIT(rgbOdomDataScan3dInfo), SYNC_INIT(rgbOdomDataScan3dInfo),
#endif
// 1 RGBD // 1 RGBD
SYNC_INIT(rgbdScan2d), SYNC_INIT(rgbdScan2d),
SYNC_INIT(rgbdScan3d), SYNC_INIT(rgbdScan3d),
@@ -130,6 +133,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbdOdomScan2dInfo), SYNC_INIT(rgbdOdomScan2dInfo),
SYNC_INIT(rgbdOdomScan3dInfo), SYNC_INIT(rgbdOdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// 1 RGBD + User Data // 1 RGBD + User Data
SYNC_INIT(rgbdData), SYNC_INIT(rgbdData),
SYNC_INIT(rgbdDataScan2d), SYNC_INIT(rgbdDataScan2d),
@@ -145,7 +149,9 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbdOdomDataInfo), SYNC_INIT(rgbdOdomDataInfo),
SYNC_INIT(rgbdOdomDataScan2dInfo), SYNC_INIT(rgbdOdomDataScan2dInfo),
SYNC_INIT(rgbdOdomDataScan3dInfo), SYNC_INIT(rgbdOdomDataScan3dInfo),
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD // 2 RGBD
SYNC_INIT(rgbd2), SYNC_INIT(rgbd2),
SYNC_INIT(rgbd2Scan2d), SYNC_INIT(rgbd2Scan2d),
@@ -162,6 +168,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd2OdomScan2dInfo), SYNC_INIT(rgbd2OdomScan2dInfo),
SYNC_INIT(rgbd2OdomScan3dInfo), SYNC_INIT(rgbd2OdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
SYNC_INIT(rgbd2Data), SYNC_INIT(rgbd2Data),
SYNC_INIT(rgbd2DataScan2d), SYNC_INIT(rgbd2DataScan2d),
@@ -177,6 +184,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd2OdomDataInfo), SYNC_INIT(rgbd2OdomDataInfo),
SYNC_INIT(rgbd2OdomDataScan2dInfo), SYNC_INIT(rgbd2OdomDataScan2dInfo),
SYNC_INIT(rgbd2OdomDataScan3dInfo), SYNC_INIT(rgbd2OdomDataScan3dInfo),
#endif
// 3 RGBD // 3 RGBD
SYNC_INIT(rgbd3), SYNC_INIT(rgbd3),
@@ -194,6 +202,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd3OdomScan2dInfo), SYNC_INIT(rgbd3OdomScan2dInfo),
SYNC_INIT(rgbd3OdomScan3dInfo), SYNC_INIT(rgbd3OdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// 3 RGBD + User Data // 3 RGBD + User Data
SYNC_INIT(rgbd3Data), SYNC_INIT(rgbd3Data),
SYNC_INIT(rgbd3DataScan2d), SYNC_INIT(rgbd3DataScan2d),
@@ -209,6 +218,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd3OdomDataInfo), SYNC_INIT(rgbd3OdomDataInfo),
SYNC_INIT(rgbd3OdomDataScan2dInfo), SYNC_INIT(rgbd3OdomDataScan2dInfo),
SYNC_INIT(rgbd3OdomDataScan3dInfo), SYNC_INIT(rgbd3OdomDataScan3dInfo),
#endif
// 4 RGBD // 4 RGBD
SYNC_INIT(rgbd4), SYNC_INIT(rgbd4),
@@ -226,6 +236,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd4OdomScan2dInfo), SYNC_INIT(rgbd4OdomScan2dInfo),
SYNC_INIT(rgbd4OdomScan3dInfo), SYNC_INIT(rgbd4OdomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// 4 RGBD + User Data // 4 RGBD + User Data
SYNC_INIT(rgbd4Data), SYNC_INIT(rgbd4Data),
SYNC_INIT(rgbd4DataScan2d), SYNC_INIT(rgbd4DataScan2d),
@@ -241,6 +252,8 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(rgbd4OdomDataInfo), SYNC_INIT(rgbd4OdomDataInfo),
SYNC_INIT(rgbd4OdomDataScan2dInfo), SYNC_INIT(rgbd4OdomDataScan2dInfo),
SYNC_INIT(rgbd4OdomDataScan3dInfo), SYNC_INIT(rgbd4OdomDataScan3dInfo),
#endif
#endif // RTABMAP_SYNC_MULTI_RGBD
// Scan // Scan
SYNC_INIT(scan2dInfo), SYNC_INIT(scan2dInfo),
@@ -252,6 +265,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(odomScan2dInfo), SYNC_INIT(odomScan2dInfo),
SYNC_INIT(odomScan3dInfo), SYNC_INIT(odomScan3dInfo),
#ifdef RTABMAP_SYNC_USER_DATA
// Scan + User Data // Scan + User Data
SYNC_INIT(dataScan2d), SYNC_INIT(dataScan2d),
SYNC_INIT(dataScan3d), SYNC_INIT(dataScan3d),
@@ -263,12 +277,16 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SYNC_INIT(odomDataScan3d), SYNC_INIT(odomDataScan3d),
SYNC_INIT(odomDataScan2dInfo), SYNC_INIT(odomDataScan2dInfo),
SYNC_INIT(odomDataScan3dInfo), SYNC_INIT(odomDataScan3dInfo),
#endif
// Odom // Odom
SYNC_INIT(odomInfo), SYNC_INIT(odomInfo)
#ifdef RTABMAP_SYNC_USER_DATA
,
// Odom + User Data // Odom + User Data
SYNC_INIT(odomData), SYNC_INIT(odomData),
SYNC_INIT(odomDataInfo) SYNC_INIT(odomDataInfo)
#endif
{ {
} }
@@ -300,6 +318,15 @@ void CommonDataSubscriber::setupCallbacks(
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData); pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
pnh.param("subscribe_odom", subscribeOdom, subscribeOdom); pnh.param("subscribe_odom", subscribeOdom, subscribeOdom);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData)
{
ROS_ERROR("subscribe_user_data is true, but rtabmap_ros has been built with RTABMAP_SYNC_USER_DATA. Setting back to false.");
subscribeUserData = false;
}
#endif
if(subscribedToDepth_ && subscribedToStereo_) if(subscribedToDepth_ && subscribedToStereo_)
{ {
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
@@ -419,6 +446,7 @@ void CommonDataSubscriber::setupCallbacks(
} }
else if(subscribedToRGBD_) else if(subscribedToRGBD_)
{ {
#ifdef RTABMAP_SYNC_MULTI_RGBD
if(rgbdCameras == 4) if(rgbdCameras == 4)
{ {
setupRGBD4Callbacks( setupRGBD4Callbacks(
@@ -458,6 +486,12 @@ void CommonDataSubscriber::setupCallbacks(
queueSize_, queueSize_,
approxSync_); approxSync_);
} }
#else
if(rgbdCameras>1)
{
ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has been built without RTABMAP_SYNC_MULTI_RGBD option)");
}
#endif
else else
{ {
setupRGBDCallbacks( setupRGBDCallbacks(
@@ -527,6 +561,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(depthOdomScan2dInfo); SYNC_DEL(depthOdomScan2dInfo);
SYNC_DEL(depthOdomScan3dInfo); SYNC_DEL(depthOdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// RGB + Depth + User Data // RGB + Depth + User Data
SYNC_DEL(depthData); SYNC_DEL(depthData);
SYNC_DEL(depthDataScan2d); SYNC_DEL(depthDataScan2d);
@@ -542,6 +577,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(depthOdomDataInfo); SYNC_DEL(depthOdomDataInfo);
SYNC_DEL(depthOdomDataScan2dInfo); SYNC_DEL(depthOdomDataScan2dInfo);
SYNC_DEL(depthOdomDataScan3dInfo); SYNC_DEL(depthOdomDataScan3dInfo);
#endif
// Stereo // Stereo
SYNC_DEL(stereo); SYNC_DEL(stereo);
@@ -567,6 +603,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbOdomScan2dInfo); SYNC_DEL(rgbOdomScan2dInfo);
SYNC_DEL(rgbOdomScan3dInfo); SYNC_DEL(rgbOdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// RGB-only + User Data // RGB-only + User Data
SYNC_DEL(rgbData); SYNC_DEL(rgbData);
SYNC_DEL(rgbDataScan2d); SYNC_DEL(rgbDataScan2d);
@@ -582,6 +619,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbOdomDataInfo); SYNC_DEL(rgbOdomDataInfo);
SYNC_DEL(rgbOdomDataScan2dInfo); SYNC_DEL(rgbOdomDataScan2dInfo);
SYNC_DEL(rgbOdomDataScan3dInfo); SYNC_DEL(rgbOdomDataScan3dInfo);
#endif
// 1 RGBD // 1 RGBD
SYNC_DEL(rgbdScan2d); SYNC_DEL(rgbdScan2d);
@@ -598,6 +636,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbdOdomScan2dInfo); SYNC_DEL(rgbdOdomScan2dInfo);
SYNC_DEL(rgbdOdomScan3dInfo); SYNC_DEL(rgbdOdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 1 RGBD + User Data // 1 RGBD + User Data
SYNC_DEL(rgbdData); SYNC_DEL(rgbdData);
SYNC_DEL(rgbdDataScan2d); SYNC_DEL(rgbdDataScan2d);
@@ -613,7 +652,9 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbdOdomDataInfo); SYNC_DEL(rgbdOdomDataInfo);
SYNC_DEL(rgbdOdomDataScan2dInfo); SYNC_DEL(rgbdOdomDataScan2dInfo);
SYNC_DEL(rgbdOdomDataScan3dInfo); SYNC_DEL(rgbdOdomDataScan3dInfo);
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD // 2 RGBD
SYNC_DEL(rgbd2); SYNC_DEL(rgbd2);
SYNC_DEL(rgbd2Scan2d); SYNC_DEL(rgbd2Scan2d);
@@ -630,6 +671,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd2OdomScan2dInfo); SYNC_DEL(rgbd2OdomScan2dInfo);
SYNC_DEL(rgbd2OdomScan3dInfo); SYNC_DEL(rgbd2OdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
SYNC_DEL(rgbd2Data); SYNC_DEL(rgbd2Data);
SYNC_DEL(rgbd2DataScan2d); SYNC_DEL(rgbd2DataScan2d);
@@ -645,6 +687,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd2OdomDataInfo); SYNC_DEL(rgbd2OdomDataInfo);
SYNC_DEL(rgbd2OdomDataScan2dInfo); SYNC_DEL(rgbd2OdomDataScan2dInfo);
SYNC_DEL(rgbd2OdomDataScan3dInfo); SYNC_DEL(rgbd2OdomDataScan3dInfo);
#endif
// 3 RGBD // 3 RGBD
SYNC_DEL(rgbd3); SYNC_DEL(rgbd3);
@@ -662,6 +705,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd3OdomScan2dInfo); SYNC_DEL(rgbd3OdomScan2dInfo);
SYNC_DEL(rgbd3OdomScan3dInfo); SYNC_DEL(rgbd3OdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 3 RGBD + User Data // 3 RGBD + User Data
SYNC_DEL(rgbd3Data); SYNC_DEL(rgbd3Data);
SYNC_DEL(rgbd3DataScan2d); SYNC_DEL(rgbd3DataScan2d);
@@ -677,6 +721,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd3OdomDataInfo); SYNC_DEL(rgbd3OdomDataInfo);
SYNC_DEL(rgbd3OdomDataScan2dInfo); SYNC_DEL(rgbd3OdomDataScan2dInfo);
SYNC_DEL(rgbd3OdomDataScan3dInfo); SYNC_DEL(rgbd3OdomDataScan3dInfo);
#endif
// 4 RGBD // 4 RGBD
SYNC_DEL(rgbd4); SYNC_DEL(rgbd4);
@@ -694,6 +739,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd4OdomScan2dInfo); SYNC_DEL(rgbd4OdomScan2dInfo);
SYNC_DEL(rgbd4OdomScan3dInfo); SYNC_DEL(rgbd4OdomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// 4 RGBD + User Data // 4 RGBD + User Data
SYNC_DEL(rgbd4Data); SYNC_DEL(rgbd4Data);
SYNC_DEL(rgbd4DataScan2d); SYNC_DEL(rgbd4DataScan2d);
@@ -709,6 +755,8 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(rgbd4OdomDataInfo); SYNC_DEL(rgbd4OdomDataInfo);
SYNC_DEL(rgbd4OdomDataScan2dInfo); SYNC_DEL(rgbd4OdomDataScan2dInfo);
SYNC_DEL(rgbd4OdomDataScan3dInfo); SYNC_DEL(rgbd4OdomDataScan3dInfo);
#endif
#endif //RTABMAP_SYNC_MULTI_RGBD
// Scan // Scan
SYNC_DEL(scan2dInfo); SYNC_DEL(scan2dInfo);
@@ -720,6 +768,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(odomScan2dInfo); SYNC_DEL(odomScan2dInfo);
SYNC_DEL(odomScan3dInfo); SYNC_DEL(odomScan3dInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// Scan + User Data // Scan + User Data
SYNC_DEL(dataScan2d); SYNC_DEL(dataScan2d);
SYNC_DEL(dataScan3d); SYNC_DEL(dataScan3d);
@@ -731,12 +780,15 @@ CommonDataSubscriber::~CommonDataSubscriber()
SYNC_DEL(odomDataScan3d); SYNC_DEL(odomDataScan3d);
SYNC_DEL(odomDataScan2dInfo); SYNC_DEL(odomDataScan2dInfo);
SYNC_DEL(odomDataScan3dInfo); SYNC_DEL(odomDataScan3dInfo);
#endif
// Odom // Odom
SYNC_DEL(odomInfo); SYNC_DEL(odomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// Odom + User Data // Odom + User Data
SYNC_DEL(odomData); SYNC_DEL(odomData);
SYNC_DEL(odomDataInfo); SYNC_DEL(odomDataInfo);
#endif
for(unsigned int i=0; i<rgbdSubs_.size(); ++i) for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
src/CoreWrapper.cpp
+4 -1
View File
@@ -241,6 +241,7 @@ void CoreWrapper::onInit()
configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir()); configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir()); databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir());
#ifndef _WIN32
if(configPath_.size() && configPath_.at(0) != '/') if(configPath_.size() && configPath_.at(0) != '/')
{ {
configPath_ = UDirectory::currentDir(true) + configPath_; configPath_ = UDirectory::currentDir(true) + configPath_;
@@ -249,6 +250,7 @@ void CoreWrapper::onInit()
{ {
databasePath_ = UDirectory::currentDir(true) + databasePath_; databasePath_ = UDirectory::currentDir(true) + databasePath_;
} }
#endif
ParametersMap allParameters = Parameters::getDefaultParameters(); ParametersMap allParameters = Parameters::getDefaultParameters();
// remove Odom parameters // remove Odom parameters
@@ -322,7 +324,7 @@ void CoreWrapper::onInit()
//parse input arguments //parse input arguments
std::vector<std::string> argList = getMyArgv(); std::vector<std::string> argList = getMyArgv();
char * argv[argList.size()]; char ** argv = new char*[argList.size()];
bool deleteDbOnStart = false; bool deleteDbOnStart = false;
for(unsigned int i=0; i<argList.size(); ++i) for(unsigned int i=0; i<argList.size(); ++i)
{ {
@@ -333,6 +335,7 @@ void CoreWrapper::onInit()
} }
} }
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv); rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
delete [] argv;
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{ {
uInsert(parameters_, ParametersPair(iter->first, iter->second)); uInsert(parameters_, ParametersPair(iter->first, iter->second));
src/DbPlayerNode.cpp
+4
View File
@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OdometryEvent.h> #include <rtabmap/core/OdometryEvent.h>
#include <cmath> #include <cmath>
#ifndef _WIN32
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <termios.h> #include <termios.h>
bool spacehit() bool spacehit()
@@ -89,6 +90,7 @@ bool spacehit()
return hit; return hit;
} }
#endif
bool paused = false; bool paused = false;
bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -637,6 +639,7 @@ int main(int argc, char** argv)
while(ros::ok()) while(ros::ok())
{ {
#ifndef _WIN32
if (spacehit()) { if (spacehit()) {
paused = !paused; paused = !paused;
if(paused) if(paused)
@@ -648,6 +651,7 @@ int main(int argc, char** argv)
ROS_INFO("resumed!"); ROS_INFO("resumed!");
} }
} }
#endif
if(!paused) if(!paused)
{ {
src/OdometryROS.cpp
+2 -1
View File
@@ -275,13 +275,14 @@ void OdometryROS::onInit()
} }
std::vector<std::string> argList = getMyArgv(); std::vector<std::string> argList = getMyArgv();
char * argv[argList.size()]; char ** argv = new char*[argList.size()];
for(unsigned int i=0; i<argList.size(); ++i) for(unsigned int i=0; i<argList.size(); ++i)
{ {
argv[i] = &argList[i].at(0); argv[i] = &argList[i].at(0);
} }
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv); rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
delete [] argv;
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{ {
rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first); rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
src/impl/CommonDataSubscriberDepth.cpp
+8 -1
View File
@@ -177,6 +177,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// RGB + Depth + User Data // RGB + Depth + User Data
void CommonDataSubscriber::depthDataCallback( void CommonDataSubscriber::depthDataCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg, const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -324,6 +325,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupDepthCallbacks( void CommonDataSubscriber::setupDepthCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -353,6 +355,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -399,7 +402,9 @@ void CommonDataSubscriber::setupDepthCallbacks(
SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -444,6 +449,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -490,6 +496,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberOdom.cpp
+6 -1
View File
@@ -47,6 +47,7 @@ void CommonDataSubscriber::odomInfoCallback(
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
void CommonDataSubscriber::odomDataCallback( void CommonDataSubscriber::odomDataCallback(
const nav_msgs::OdometryConstPtr& odomMsg, const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg) const rtabmap_ros::UserDataConstPtr & userDataMsg)
@@ -64,6 +65,7 @@ void CommonDataSubscriber::odomDataInfoCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg); commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupOdomCallbacks( void CommonDataSubscriber::setupOdomCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -79,6 +81,7 @@ void CommonDataSubscriber::setupOdomCallbacks(
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData) if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -93,7 +96,9 @@ void CommonDataSubscriber::setupOdomCallbacks(
SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_); SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_);
} }
} }
else if(subscribeOdomInfo) else
#endif
if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1); odomInfoSub_.subscribe(nh, "odom_info", 1);
src/impl/CommonDataSubscriberRGB.cpp
+8 -1
View File
@@ -177,6 +177,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// RGB + Depth + User Data // RGB + Depth + User Data
void CommonDataSubscriber::rgbDataCallback( void CommonDataSubscriber::rgbDataCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg, const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -324,6 +325,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
cv_bridge::CvImageConstPtr depthMsg;// Null cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupRGBCallbacks( void CommonDataSubscriber::setupRGBCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -347,6 +349,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -393,7 +396,9 @@ void CommonDataSubscriber::setupRGBCallbacks(
SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -438,6 +443,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_); SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -484,6 +490,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_); SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberRGBD.cpp
+8 -2
View File
@@ -192,6 +192,7 @@ void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// 1 RGBD camera + User Data // 1 RGBD camera + User Data
void CommonDataSubscriber::rgbdDataCallback( void CommonDataSubscriber::rgbdDataCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg, const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -351,6 +352,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scanMsg; // Null sensor_msgs::LaserScanConstPtr scanMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg); commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupRGBDCallbacks( void CommonDataSubscriber::setupRGBDCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -371,7 +373,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>; rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1); rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -417,7 +419,9 @@ void CommonDataSubscriber::setupRGBDCallbacks(
SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0])); SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d) if(subscribeScan2d)
@@ -461,6 +465,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0])); SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -505,6 +510,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberRGBD2.cpp
+8 -1
View File
@@ -202,6 +202,7 @@ void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
void CommonDataSubscriber::rgbd2DataCallback( void CommonDataSubscriber::rgbd2DataCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg, const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -361,6 +362,7 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scanMsg; // Null sensor_msgs::LaserScanConstPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupRGBD2Callbacks( void CommonDataSubscriber::setupRGBD2Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -381,6 +383,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>; rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1); rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
} }
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -426,7 +429,9 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d) if(subscribeScan2d)
@@ -470,6 +475,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -514,6 +520,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberRGBD3.cpp
+8 -1
View File
@@ -216,6 +216,7 @@ void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
void CommonDataSubscriber::rgbd3DataCallback( void CommonDataSubscriber::rgbd3DataCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg, const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -387,6 +388,7 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scanMsg; // Null sensor_msgs::LaserScanConstPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupRGBD3Callbacks( void CommonDataSubscriber::setupRGBD3Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -407,6 +409,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>; rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1); rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
} }
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -452,7 +455,9 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
SYNC_DECL5(rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); SYNC_DECL5(rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d) if(subscribeScan2d)
@@ -496,6 +501,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -540,6 +546,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
SYNC_DECL4(rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); SYNC_DECL4(rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberRGBD4.cpp
+8 -1
View File
@@ -230,6 +230,7 @@ void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data // 2 RGBD + User Data
void CommonDataSubscriber::rgbd4DataCallback( void CommonDataSubscriber::rgbd4DataCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg, const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -413,6 +414,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scanMsg; // Null sensor_msgs::LaserScanConstPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg); commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupRGBD4Callbacks( void CommonDataSubscriber::setupRGBD4Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -433,6 +435,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>; rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1); rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
} }
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -478,7 +481,9 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
SYNC_DECL6(rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); SYNC_DECL6(rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d) if(subscribeScan2d)
@@ -522,6 +527,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
SYNC_DECL5(rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); SYNC_DECL5(rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -566,6 +572,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
SYNC_DECL5(rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); SYNC_DECL5(rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
} }
} }
#endif
else else
{ {
if(subscribeScan2d) if(subscribeScan2d)
src/impl/CommonDataSubscriberScan.cpp
+8 -1
View File
@@ -111,6 +111,7 @@ void CommonDataSubscriber::odomScan3dInfoCallback(
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg); commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#ifdef RTABMAP_SYNC_USER_DATA
void CommonDataSubscriber::dataScan2dCallback( void CommonDataSubscriber::dataScan2dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg, const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg) const sensor_msgs::LaserScanConstPtr& scanMsg)
@@ -192,6 +193,7 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback(
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg); commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
} }
#endif
void CommonDataSubscriber::setupScanCallbacks( void CommonDataSubscriber::setupScanCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -218,6 +220,7 @@ void CommonDataSubscriber::setupScanCallbacks(
scan3dSub_.subscribe(nh, "scan_cloud", 1); scan3dSub_.subscribe(nh, "scan_cloud", 1);
} }
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -250,7 +253,9 @@ void CommonDataSubscriber::setupScanCallbacks(
} }
} }
} }
else if(subscribeOdom) else
#endif
if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
@@ -281,6 +286,7 @@ void CommonDataSubscriber::setupScanCallbacks(
} }
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", 1); userDataSub_.subscribe(nh, "user_data", 1);
@@ -312,6 +318,7 @@ void CommonDataSubscriber::setupScanCallbacks(
} }
} }
} }
#endif
else else
{ {
if(scan2dTopic) if(scan2dTopic)