feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,179 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(rtabmap_sync)
|
||||
|
||||
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
|
||||
# issues #1285 #1288
|
||||
find_library(
|
||||
builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
|
||||
PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
|
||||
)
|
||||
find_library(
|
||||
crypto_LIB NAMES crypto
|
||||
PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
|
||||
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
|
||||
)
|
||||
endif()
|
||||
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_transport REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
find_package(rtabmap_conversions REQUIRED)
|
||||
find_package(rtabmap_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(diagnostic_updater REQUIRED)
|
||||
|
||||
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}")
|
||||
MESSAGE(STATUS "RTABMAP_SYNC_USER_DATA = ${RTABMAP_SYNC_USER_DATA}")
|
||||
IF(RTABMAP_SYNC_MULTI_RGBD)
|
||||
add_definitions("-DRTABMAP_SYNC_MULTI_RGBD")
|
||||
SET(RTABMAP_SYNC_MULTI_RGBD_CONF 1)
|
||||
ELSE()
|
||||
SET(RTABMAP_SYNC_MULTI_RGBD_CONF 0)
|
||||
ENDIF()
|
||||
IF(RTABMAP_SYNC_USER_DATA)
|
||||
add_definitions("-DRTABMAP_SYNC_USER_DATA")
|
||||
SET(RTABMAP_SYNC_USER_DATA_CONF 1)
|
||||
ELSE()
|
||||
SET(RTABMAP_SYNC_USER_DATA_CONF 0)
|
||||
ENDIF()
|
||||
|
||||
IF(WIN32)
|
||||
add_compile_options(-bigobj)
|
||||
ENDIF(WIN32)
|
||||
|
||||
configure_file(cmake/extra_configs.cmake.in cmake/extra_configs.cmake IMMEDIATE @ONLY)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
# libraries
|
||||
SET(Libraries
|
||||
cv_bridge
|
||||
image_transport
|
||||
message_filters
|
||||
nav_msgs
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
rtabmap_conversions
|
||||
rtabmap_msgs
|
||||
sensor_msgs
|
||||
diagnostic_updater
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
SET(rtabmap_sync_lib_src
|
||||
src/CommonDataSubscriber.cpp
|
||||
src/impl/CommonDataSubscriberDepth.cpp
|
||||
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/impl/CommonDataSubscriberSensorData.cpp
|
||||
)
|
||||
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
|
||||
src/impl/CommonDataSubscriberRGBD5.cpp
|
||||
src/impl/CommonDataSubscriberRGBD6.cpp
|
||||
)
|
||||
ENDIF(RTABMAP_SYNC_MULTI_RGBD)
|
||||
|
||||
SET(rtabmap_sync_plugins_lib_src
|
||||
src/nodelets/rgbdx_sync.cpp
|
||||
src/nodelets/rgbd_sync.cpp
|
||||
src/nodelets/stereo_sync.cpp
|
||||
src/nodelets/rgb_sync.cpp
|
||||
)
|
||||
|
||||
|
||||
############################
|
||||
## Declare a cpp library
|
||||
############################
|
||||
add_library(rtabmap_sync SHARED
|
||||
${rtabmap_sync_lib_src}
|
||||
)
|
||||
add_library(rtabmap_sync_plugins SHARED
|
||||
${rtabmap_sync_plugins_lib_src}
|
||||
)
|
||||
|
||||
target_include_directories(rtabmap_sync
|
||||
PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
ament_target_dependencies(rtabmap_sync ${Libraries})
|
||||
ament_target_dependencies(rtabmap_sync_plugins ${Libraries})
|
||||
|
||||
rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDSync")
|
||||
rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::StereoSync")
|
||||
rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBSync")
|
||||
rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDXSync")
|
||||
|
||||
add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp)
|
||||
ament_target_dependencies(rtabmap_rgbd_sync ${Libraries})
|
||||
target_link_libraries(rtabmap_rgbd_sync rtabmap_sync_plugins)
|
||||
set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync")
|
||||
|
||||
add_executable(rtabmap_rgbdx_sync src/RGBDXSyncNode.cpp)
|
||||
ament_target_dependencies(rtabmap_rgbdx_sync ${Libraries})
|
||||
target_link_libraries(rtabmap_rgbdx_sync rtabmap_sync_plugins)
|
||||
set_target_properties(rtabmap_rgbdx_sync PROPERTIES OUTPUT_NAME "rgbdx_sync")
|
||||
|
||||
add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp)
|
||||
ament_target_dependencies(rtabmap_stereo_sync ${Libraries})
|
||||
target_link_libraries(rtabmap_stereo_sync rtabmap_sync_plugins)
|
||||
set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync")
|
||||
|
||||
add_executable(rtabmap_rgb_sync src/RGBSyncNode.cpp)
|
||||
ament_target_dependencies(rtabmap_rgb_sync ${Libraries})
|
||||
target_link_libraries(rtabmap_rgb_sync rtabmap_sync_plugins)
|
||||
set_target_properties(rtabmap_rgb_sync PROPERTIES OUTPUT_NAME "rgb_sync")
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
ament_export_dependencies(${Libraries})
|
||||
ament_export_include_directories(include)
|
||||
ament_export_targets(${PROJECT_NAME}) # To include downstream with targets
|
||||
ament_export_libraries(rtabmap_sync rtabmap_sync_plugins) # To include downstream without targets
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_sync
|
||||
rtabmap_sync_plugins
|
||||
EXPORT ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
rtabmap_rgbd_sync
|
||||
rtabmap_rgbdx_sync
|
||||
rtabmap_stereo_sync
|
||||
rtabmap_rgb_sync
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
ament_package(CONFIG_EXTRAS ${CMAKE_CURRENT_BINARY_DIR}/cmake/extra_configs.cmake)
|
||||
@@ -0,0 +1,8 @@
|
||||
|
||||
if(@RTABMAP_SYNC_MULTI_RGBD_CONF@)
|
||||
add_definitions("-DRTABMAP_SYNC_MULTI_RGBD")
|
||||
endif()
|
||||
|
||||
if(@RTABMAP_SYNC_USER_DATA_CONF@)
|
||||
add_definitions("-DRTABMAP_SYNC_USER_DATA")
|
||||
endif()
|
||||
@@ -0,0 +1,636 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
|
||||
#include <rtabmap_sync/visibility.h>
|
||||
#include <message_filters/subscriber.hpp>
|
||||
#include <message_filters/synchronizer.hpp>
|
||||
#include <message_filters/sync_policies/approximate_time.hpp>
|
||||
#include <message_filters/sync_policies/exact_time.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
#include <rtabmap_msgs/msg/user_data.hpp>
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
|
||||
#include <rtabmap_msgs/msg/sensor_data.hpp>
|
||||
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
|
||||
#include <rtabmap_sync/SyncDiagnostic.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
class CommonDataSubscriber {
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
CommonDataSubscriber(rclcpp::Node & node, bool gui);
|
||||
virtual ~CommonDataSubscriber();
|
||||
|
||||
bool isSubscribedToDepth() const {return subscribedToDepth_;}
|
||||
bool isSubscribedToStereo() const {return subscribedToStereo_;}
|
||||
bool isSubscribedToRGB() const {return subscribedToRGB_;}
|
||||
bool isSubscribedToOdom() const {return subscribedToOdom_;}
|
||||
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
|
||||
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
|
||||
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
|
||||
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
|
||||
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
|
||||
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
|
||||
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
|
||||
int getTopicQueueSize() const {return topicQueueSize_;}
|
||||
int getSyncQueueSize() const {return syncQueueSize_;}
|
||||
bool isApproxSync() const {return approxSync_;}
|
||||
const std::string & name() const {return name_;}
|
||||
|
||||
protected:
|
||||
void setupCallbacks(
|
||||
rclcpp::Node & node,
|
||||
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
|
||||
virtual void commonMultiCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
|
||||
const sensor_msgs::msg::LaserScan& scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2& scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
|
||||
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
|
||||
virtual void commonLaserScanCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()) = 0;
|
||||
virtual void commonOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
|
||||
virtual void commonSensorDataCallback(
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
|
||||
|
||||
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
|
||||
|
||||
private:
|
||||
void commonSingleCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr & imageMsg,
|
||||
const cv_bridge::CvImageConstPtr & depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
|
||||
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
|
||||
const cv::Mat & localDescriptors = cv::Mat());
|
||||
void processSyncData();
|
||||
void setupDepthCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupStereoCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDXCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
#ifdef RTABMAP_SYNC_MULTI_RGBD
|
||||
void setupRGBD2Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD3Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD4Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD5Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD6Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo);
|
||||
#endif
|
||||
void setupSensorDataCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupScanCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo);
|
||||
void setupOdomCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo);
|
||||
|
||||
protected:
|
||||
std::string subscribedTopicsMsg_;
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
rmw_qos_reliability_policy_t qosOdom_;
|
||||
rmw_qos_reliability_policy_t qosImage_;
|
||||
rmw_qos_reliability_policy_t qosCameraInfo_;
|
||||
rmw_qos_reliability_policy_t qosScan_;
|
||||
rmw_qos_reliability_policy_t qosUserData_;
|
||||
rmw_qos_reliability_policy_t qosSensorData_;
|
||||
|
||||
private:
|
||||
bool approxSync_;
|
||||
bool subscribedToDepth_;
|
||||
bool subscribedToStereo_;
|
||||
bool subscribedToRGB_;
|
||||
bool subscribedToOdom_;
|
||||
bool subscribedToRGBD_;
|
||||
bool subscribedToSensorData_;
|
||||
bool subscribedToScan2d_;
|
||||
bool subscribedToScan3d_;
|
||||
bool subscribedToScanDescriptor_;
|
||||
bool subscribedToOdomInfo_;
|
||||
bool subscribedToUserData_;
|
||||
std::string odomFrameId_;
|
||||
int rgbdCameras_;
|
||||
std::string name_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_;
|
||||
|
||||
//for depth and rgb-only callbacks
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
//for rgbd callback
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::ConstSharedPtr rgbdSub_;
|
||||
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::ConstSharedPtr rgbdXSubOnly_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImages> rgbdXSub_;
|
||||
|
||||
//for sensor data callback
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::SensorData>::ConstSharedPtr sensorDataSubOnly_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::SensorData> sensorDataSub_;
|
||||
|
||||
//stereo callback
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
|
||||
|
||||
message_filters::Subscriber<nav_msgs::msg::Odometry> odomSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::UserData> userDataSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::LaserScan> scanSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> scan3dSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::ScanDescriptor> scanDescSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::OdomInfo> odomInfoSub_;
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::ConstSharedPtr scan2dSubOnly_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::ConstSharedPtr scan3dSubOnly_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::ScanDescriptor>::ConstSharedPtr scanDescSubOnly_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::ConstSharedPtr odomSubOnly_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
|
||||
// RGB + Depth
|
||||
DATA_SYNCS3(depth, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(depthScan2d, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(depthScan3d, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(depthScanDesc, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(depthInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScan2dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScan3dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(depthScanDescInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB + Depth + Odom
|
||||
DATA_SYNCS4(depthOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(depthOdomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(depthOdomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(depthOdomScanDesc, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(depthOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthOdomScanDescInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB + Depth + User Data
|
||||
DATA_SYNCS4(depthData, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(depthDataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(depthDataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(depthDataScanDesc, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(depthDataInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(depthDataScanDescInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
DATA_SYNCS5(depthOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(depthOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(depthOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS7(depthOdomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// Stereo
|
||||
DATA_SYNCS4(stereo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(stereoInfo, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Stereo + Odom
|
||||
DATA_SYNCS5(stereoOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS6(stereoOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only
|
||||
DATA_SYNCS2(rgb, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS3(rgbScan2d, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbScan3d, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbScanDesc, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScan2dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScan3dInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(rgbScanDescInfo, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only + Odom
|
||||
DATA_SYNCS3(rgbOdom, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(rgbOdomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbOdomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbOdomScanDesc, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbOdomInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbOdomScanDescInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB-only + User Data
|
||||
DATA_SYNCS3(rgbData, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS4(rgbDataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbDataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbDataScanDesc, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbDataInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS5(rgbDataScanDescInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// RGB-only + Odom + User Data
|
||||
DATA_SYNCS4(rgbOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
|
||||
DATA_SYNCS5(rgbOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS6(rgbOdomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 1 RGBD
|
||||
void rgbdCallback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr);
|
||||
DATA_SYNCS2(rgbdScan2d, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(rgbdScan3d, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(rgbdScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS2(rgbdInfo, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 1 RGBD + Odom
|
||||
DATA_SYNCS2(rgbdOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdOdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 1 RGBD + User Data
|
||||
DATA_SYNCS2(rgbdData, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbdDataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdDataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdDataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdDataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 1 RGBD + Odom + User Data
|
||||
DATA_SYNCS3(rgbdOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbdOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// X RGBD
|
||||
void rgbdXCallback(const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr);
|
||||
DATA_SYNCS2(rgbdXScan2d, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(rgbdXScan3d, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(rgbdXScanDesc, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS2(rgbdXInfo, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// X RGBD + Odom
|
||||
DATA_SYNCS2(rgbdXOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS3(rgbdXOdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdXOdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdXOdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdXOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// X RGBD + User Data
|
||||
DATA_SYNCS2(rgbdXData, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS3(rgbdXDataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbdXDataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbdXDataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbdXDataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// X RGBD + Odom + User Data
|
||||
DATA_SYNCS3(rgbdXOdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages)
|
||||
DATA_SYNCS4(rgbdXOdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbdXOdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbdXOdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// SensorData
|
||||
void sensorDataCallback(const rtabmap_msgs::msg::SensorData::ConstSharedPtr);
|
||||
DATA_SYNCS2(sensorDataInfo, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
|
||||
|
||||
// SensorData + Odom
|
||||
DATA_SYNCS2(sensorDataOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData);
|
||||
DATA_SYNCS3(sensorDataOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
|
||||
|
||||
#ifdef RTABMAP_SYNC_MULTI_RGBD
|
||||
// 2 RGBD
|
||||
DATA_SYNCS2(rgbd2, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbd2Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(rgbd2Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(rgbd2ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(rgbd2Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
// 2 RGBD + Odom
|
||||
DATA_SYNCS3(rgbd2Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd2OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
DATA_SYNCS3(rgbd2Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd2DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd2DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd2DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd2DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
DATA_SYNCS4(rgbd2OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd2OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 3 RGBD
|
||||
DATA_SYNCS3(rgbd3, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd3Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS4(rgbd3Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS4(rgbd3ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(rgbd3Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 3 RGBD + Odom
|
||||
DATA_SYNCS4(rgbd3Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd3OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd3OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd3OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd3OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 3 RGBD + User Data
|
||||
DATA_SYNCS4(rgbd3Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd3DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd3DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd3DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd3DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 3 RGBD + Odom + User Data
|
||||
DATA_SYNCS5(rgbd3OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd3OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd3OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd3OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 4 RGBD
|
||||
DATA_SYNCS4(rgbd4, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd4Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS5(rgbd4Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS5(rgbd4ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS5(rgbd4Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 4 RGBD + Odom
|
||||
DATA_SYNCS5(rgbd4Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd4OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd4OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd4OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd4OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 4 RGBD + User Data
|
||||
DATA_SYNCS5(rgbd4Data, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd4DataScan2d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd4DataScan3d, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd4DataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd4DataInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
// 4 RGBD + Odom + User Data
|
||||
DATA_SYNCS6(rgbd4OdomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd4OdomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd4OdomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd4OdomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// 5 RGBD
|
||||
DATA_SYNCS5(rgbd5, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd5Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS6(rgbd5Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS6(rgbd5ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS6(rgbd5Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 5 RGBD + Odom
|
||||
DATA_SYNCS6(rgbd5Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd5OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd5OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd5OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd5OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 6 RGBD
|
||||
DATA_SYNCS6(rgbd6, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd6Scan2d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS7(rgbd6Scan3d, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS7(rgbd6ScanDesc, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS7(rgbd6Info, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// 6 RGBD + Odom
|
||||
DATA_SYNCS7(rgbd6Odom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS8(rgbd6OdomScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS8(rgbd6OdomScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS8(rgbd6OdomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS8(rgbd6OdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#endif //RTABMAP_SYNC_MULTI_RGBD
|
||||
|
||||
// Scan
|
||||
void scan2dCallback(const sensor_msgs::msg::LaserScan::ConstSharedPtr);
|
||||
void scan3dCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr);
|
||||
void scanDescCallback(const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr);
|
||||
DATA_SYNCS2(scan2dInfo, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS2(scan3dInfo, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS2(scanDescInfo, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Scan + Odom
|
||||
DATA_SYNCS2(odomScan2d, nav_msgs::msg::Odometry, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(odomScan3d, nav_msgs::msg::Odometry, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(odomScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(odomScan2dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(odomScan3dInfo, nav_msgs::msg::Odometry, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(odomScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// Scan + User Data
|
||||
DATA_SYNCS2(dataScan2d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS2(dataScan3d, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS2(dataScanDesc, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS3(dataScan2dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(dataScan3dInfo, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS3(dataScanDescInfo, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
// Scan + Odom + User Data
|
||||
DATA_SYNCS3(odomDataScan2d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan)
|
||||
DATA_SYNCS3(odomDataScan3d, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2)
|
||||
DATA_SYNCS3(odomDataScanDesc, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor)
|
||||
DATA_SYNCS4(odomDataScan2dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::LaserScan, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(odomDataScan3dInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, sensor_msgs::msg::PointCloud2, rtabmap_msgs::msg::OdomInfo)
|
||||
DATA_SYNCS4(odomDataScanDescInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::ScanDescriptor, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
|
||||
// Odom
|
||||
void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr);
|
||||
DATA_SYNCS2(odomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo)
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// Odom + User Data
|
||||
DATA_SYNCS2(odomData, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData)
|
||||
DATA_SYNCS3(odomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::OdomInfo)
|
||||
#endif
|
||||
};
|
||||
|
||||
} /* namespace rtabmap_ros */
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */
|
||||
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap_sync/GetTopicName.h>
|
||||
|
||||
#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC7(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS7(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
DATA_SYNC7(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
DATA_SYNC7(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr, const MSG6 ::ConstSharedPtr);
|
||||
|
||||
#define DATA_SYNC8(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS8(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
DATA_SYNC8(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
DATA_SYNC8(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6, MSG7) \
|
||||
void PREFIX##Callback(const MSG0 ::ConstSharedPtr, const MSG1 ::ConstSharedPtr, const MSG2 ::ConstSharedPtr, const MSG3 ::ConstSharedPtr, const MSG4 ::ConstSharedPtr, const MSG5 ::ConstSharedPtr, const MSG6 ::ConstSharedPtr, const MSG7 ::ConstSharedPtr);
|
||||
|
||||
|
||||
// Constructor
|
||||
#define SYNC_INIT(PREFIX) \
|
||||
PREFIX##ApproximateSync_(0), \
|
||||
PREFIX##ExactSync_(0)
|
||||
|
||||
// Destructor
|
||||
#define SYNC_DEL(PREFIX) \
|
||||
if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \
|
||||
if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
|
||||
|
||||
// Sync declarations
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); \
|
||||
} \
|
||||
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
|
||||
name_.c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); \
|
||||
} \
|
||||
else \
|
||||
{ \
|
||||
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
|
||||
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
|
||||
PREFIX##ExactSync_->registerCallback(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_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(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB6.getSubscriber()).c_str());
|
||||
|
||||
#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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_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(std::bind(&CLASS::PREFIX##Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_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(), \
|
||||
APPROX?"approx":"exact", \
|
||||
getTopicName(SUB0.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB1.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB2.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB3.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB4.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB5.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB6.getSubscriber()).c_str(), \
|
||||
getTopicName(SUB7.getSubscriber()).c_str());
|
||||
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* GetTopicName.h
|
||||
*
|
||||
* Created on: Oct 1, 2021
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
|
||||
#define INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
template<class T>
|
||||
auto getTopicNameImpl(T const& obj, int)
|
||||
-> decltype(obj->get_topic_name(), std::string())
|
||||
{
|
||||
return obj->get_topic_name();
|
||||
}
|
||||
|
||||
template<class T>
|
||||
auto getTopicNameImpl(T const& obj, long)
|
||||
-> decltype(obj.getTopic(), std::string())
|
||||
{
|
||||
return obj.getTopic();
|
||||
}
|
||||
|
||||
template<class T>
|
||||
auto getTopicName(T const& obj)
|
||||
-> decltype(getTopicNameImpl(obj, 0), std::string())
|
||||
{
|
||||
return getTopicNameImpl(obj, 0);
|
||||
}
|
||||
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ */
|
||||
@@ -0,0 +1,182 @@
|
||||
#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
|
||||
#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
|
||||
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
#include <diagnostic_updater/diagnostic_updater.hpp>
|
||||
#include <diagnostic_updater/publisher.hpp>
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UMutex.h"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
class SyncDiagnostic {
|
||||
public:
|
||||
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) :
|
||||
node_(node),
|
||||
diagnosticUpdater_(node, 2.0),
|
||||
inFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&inTargetFrequency_, &inTargetFrequency_, tolerance), node->get_clock()),
|
||||
inTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
outFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&outTargetFrequency_, &outTargetFrequency_, tolerance), node->get_clock()),
|
||||
outTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
inCompositeTask_("Input Status"),
|
||||
outCompositeTask_("Output Status"),
|
||||
lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
|
||||
inTargetFrequency_(0.0),
|
||||
outTargetFrequency_(0.0),
|
||||
windowSize_(windowSize),
|
||||
lastTickTime_(0.0)
|
||||
{
|
||||
UASSERT(windowSize_ >= 1);
|
||||
}
|
||||
|
||||
void init(
|
||||
const std::string & topic,
|
||||
const std::string & topicsNotReceivedWarningMsg,
|
||||
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
|
||||
{
|
||||
topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
|
||||
|
||||
std::list<std::string> strList = uSplit(topic, '/');
|
||||
for(int i=0; i<2 && strList.size()>1; ++i)
|
||||
{
|
||||
// Assuming format is /back_camera/left/image, we want "back_camera"
|
||||
strList.pop_back();
|
||||
}
|
||||
inCompositeTask_.addTask(&inFrequencyStatus_);
|
||||
inCompositeTask_.addTask(&inTimeStampStatus_);
|
||||
diagnosticUpdater_.add(inCompositeTask_);
|
||||
outCompositeTask_.addTask(&outFrequencyStatus_);
|
||||
outCompositeTask_.addTask(&outTimeStampStatus_);
|
||||
diagnosticUpdater_.add(outCompositeTask_);
|
||||
for(size_t i=0; i<otherTasks.size(); ++i)
|
||||
{
|
||||
diagnosticUpdater_.add(*otherTasks[i]);
|
||||
}
|
||||
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
|
||||
diagnosticUpdater_.force_update();
|
||||
diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
|
||||
}
|
||||
|
||||
void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
inFrequencyStatus_,
|
||||
inTimeStampStatus_,
|
||||
inWindow_,
|
||||
inTargetFrequency_,
|
||||
lastTickInputStamp_);
|
||||
}
|
||||
|
||||
void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
double lastTickOutputStamp;
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
outFrequencyStatus_,
|
||||
outTimeStampStatus_,
|
||||
outWindow_,
|
||||
outTargetFrequency_,
|
||||
lastTickOutputStamp);
|
||||
}
|
||||
|
||||
private:
|
||||
void diagnosticTimerCallback()
|
||||
{
|
||||
UScopeMutex lock(tickMutex_);
|
||||
if(rtabmap_conversions::timestampFromROS(node_->now())-lastTickInputStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
|
||||
{
|
||||
RCLCPP_WARN(node_->get_logger(), "%s", topicsNotReceivedWarningMsg_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void updateFrequency(
|
||||
const rclcpp::Time & stamp,
|
||||
const double & expectedFrequency,
|
||||
diagnostic_updater::FrequencyStatus & freqStatus,
|
||||
diagnostic_updater::TimeStampStatus & timeStatus,
|
||||
std::deque<double> & window,
|
||||
double & targetFrequency,
|
||||
double & lastTickStamp)
|
||||
{
|
||||
UScopeMutex lock(tickMutex_);
|
||||
|
||||
freqStatus.tick();
|
||||
timeStatus.tick(stamp);
|
||||
|
||||
double stampSec = rtabmap_conversions::timestampFromROS(stamp);
|
||||
double singlePeriod = stampSec - lastTickStamp;
|
||||
|
||||
window.push_back(singlePeriod);
|
||||
if(window.size() > windowSize_)
|
||||
{
|
||||
window.pop_front();
|
||||
|
||||
double period = 0.0;
|
||||
if(window.size() == windowSize_)
|
||||
{
|
||||
for(size_t i=0; i<window.size(); ++i)
|
||||
{
|
||||
period += window[i];
|
||||
}
|
||||
period /= windowSize_;
|
||||
}
|
||||
|
||||
if(period>0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency))
|
||||
{
|
||||
targetFrequency = 1.0/period;
|
||||
}
|
||||
else if(expectedFrequency>0)
|
||||
{
|
||||
targetFrequency = expectedFrequency;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
lastTickStamp = stampSec;
|
||||
|
||||
double clockNow = rtabmap_conversions::timestampFromROS(node_->now());
|
||||
if(lastTickTime_ > clockNow)
|
||||
{
|
||||
RCLCPP_WARN(node_->get_logger(), "%s: Detected time jump in the past of %f sec, forcing diagnostic update.",
|
||||
node_->get_name(), lastTickTime_ - clockNow);
|
||||
inFrequencyStatus_.clear();
|
||||
outFrequencyStatus_.clear();
|
||||
diagnosticUpdater_.force_update();
|
||||
lastTickInputStamp_ = clockNow;
|
||||
}
|
||||
lastTickTime_ = clockNow;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
std::string topicsNotReceivedWarningMsg_;
|
||||
diagnostic_updater::Updater diagnosticUpdater_;
|
||||
diagnostic_updater::FrequencyStatus inFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus inTimeStampStatus_;
|
||||
diagnostic_updater::FrequencyStatus outFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus outTimeStampStatus_;
|
||||
diagnostic_updater::CompositeDiagnosticTask inCompositeTask_;
|
||||
diagnostic_updater::CompositeDiagnosticTask outCompositeTask_;
|
||||
rclcpp::TimerBase::SharedPtr diagnosticTimer_;
|
||||
double lastTickInputStamp_;
|
||||
double inTargetFrequency_;
|
||||
double outTargetFrequency_;
|
||||
int windowSize_;
|
||||
std::deque<double> inWindow_;
|
||||
std::deque<double> outWindow_;
|
||||
UMutex tickMutex_;
|
||||
double lastTickTime_;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
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_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
double compressedRate_;
|
||||
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
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_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBDSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBDSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBDSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
double depthScale_;
|
||||
int decimation_;
|
||||
double compressedRate_;
|
||||
double approxSyncMaxInterval_;
|
||||
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
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_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_msgs/msg/rgbd_images.hpp"
|
||||
#include "rtabmap_sync/CommonDataSubscriber.h"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class RGBDXSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit RGBDXSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~RGBDXSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
private:
|
||||
DATA_SYNCS2(rgbd2, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS3(rgbd3, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS4(rgbd4, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS5(rgbd5, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS6(rgbd6, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS7(rgbd7, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
DATA_SYNCS8(rgbd8, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdImagesPub_;
|
||||
|
||||
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
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_sync/visibility.h>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include "rtabmap_msgs/msg/rgbd_image.hpp"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
class StereoSync : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RTABMAP_SYNC_PUBLIC
|
||||
explicit StereoSync(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~StereoSync();
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
|
||||
private:
|
||||
double compressedRate_;
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageLeftSub_;
|
||||
image_transport::SubscriberFilter imageRightSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeftSub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRightSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
#define RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define RTABMAP_SYNC_EXPORT __attribute__ ((dllexport))
|
||||
#define RTABMAP_SYNC_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define RTABMAP_SYNC_EXPORT __declspec(dllexport)
|
||||
#define RTABMAP_SYNC_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef RTABMAP_SYNC_BUILDING_DLL
|
||||
#define RTABMAP_SYNC_PUBLIC RTABMAP_SYNC_EXPORT
|
||||
#else
|
||||
#define RTABMAP_SYNC_PUBLIC RTABMAP_SYNC_IMPORT
|
||||
#endif
|
||||
#define RTABMAP_SYNC_PUBLIC_TYPE RTABMAP_SYNC_PUBLIC
|
||||
#define RTABMAP_SYNC_LOCAL
|
||||
#else
|
||||
#define RTABMAP_SYNC_EXPORT __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_SYNC_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define RTABMAP_SYNC_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_SYNC_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define RTABMAP_SYNC_PUBLIC
|
||||
#define RTABMAP_SYNC_LOCAL
|
||||
#endif
|
||||
#define RTABMAP_SYNC_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // RTABMAP_SYNC__VISIBILITY_CONTROL_H_
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_sync</name>
|
||||
<version>0.22.0</version>
|
||||
<description>RTAB-Map's synchronization package.</description>
|
||||
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
<license>BSD</license>
|
||||
<url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
|
||||
<url type="repository">https://github.com/introlab/rtabmap_ros</url>
|
||||
|
||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
||||
|
||||
<build_depend>ros_environment</build_depend>
|
||||
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>rtabmap_conversions</depend>
|
||||
<depend>rtabmap_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>diagnostic_updater</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
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 <memory>
|
||||
#include "rtabmap_sync/rgbd_sync.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<rtabmap_sync::RGBDSync>(rclcpp::NodeOptions()));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
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 <memory>
|
||||
#include "rtabmap_sync/rgbdx_sync.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
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 <memory>
|
||||
#include "rtabmap_sync/rgb_sync.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<rtabmap_sync::RGBSync>(rclcpp::NodeOptions()));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
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 <memory>
|
||||
#include "rtabmap_sync/stereo_sync.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<rtabmap_sync::StereoSync>(rclcpp::NodeOptions()));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,759 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
// RGB + Depth
|
||||
void CommonDataSubscriber::depthCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan2dCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan3dCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScanDescCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::depthInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan2dInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan3dInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScanDescInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom
|
||||
void CommonDataSubscriber::depthOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB + Depth + User Data
|
||||
void CommonDataSubscriber::depthDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan2dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan3dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
void CommonDataSubscriber::depthOdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupDepthCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
|
||||
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,116 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
void CommonDataSubscriber::odomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
void CommonDataSubscriber::odomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupOdomCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
|
||||
|
||||
if(subscribeUserData || subscribeOdomInfo)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
odomSubOnly_->get_topic_name());
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,758 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
// RGB
|
||||
void CommonDataSubscriber::rgbCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan2dCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan3dCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScanDescCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::rgbInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan2dInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan3dInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScanDescInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
// RGB + Odom
|
||||
void CommonDataSubscriber::rgbOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// RGB + Depth + User Data
|
||||
void CommonDataSubscriber::rgbDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan2dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan3dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
void CommonDataSubscriber::rgbOdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptor.push_back(scanMsg->global_descriptor);
|
||||
}
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
|
||||
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,786 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
// 1 RGBD camera
|
||||
void CommonDataSubscriber::rgbdCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
*scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, *scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom
|
||||
void CommonDataSubscriber::rgbdOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
*scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, *scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 1 RGBD camera + User Data
|
||||
void CommonDataSubscriber::rgbdDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
*scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, *scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom + User Data
|
||||
void CommonDataSubscriber::rgbdOdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
*scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, *scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs;
|
||||
if(!image1Msg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
|
||||
}
|
||||
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
|
||||
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
|
||||
scanMsg, scan3dMsg, odomInfoMsg,
|
||||
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
|
||||
rtabmap::uncompressData(image1Msg->descriptors));
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd callback");
|
||||
|
||||
if(subscribeOdom ||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
subscribeUserData ||
|
||||
#endif
|
||||
subscribeScan2d ||
|
||||
subscribeScan3d ||
|
||||
subscribeScanDesc ||
|
||||
subscribeOdomInfo)
|
||||
{
|
||||
rgbdSubs_.resize(1);
|
||||
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]));
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]));
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Not supposed to be here!");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = node.create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
rgbdSub_->get_topic_name());
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,562 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
if(!image1Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
|
||||
if(!image2Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
|
||||
localKeyPoints.push_back(image1Msg->key_points); \
|
||||
localKeyPoints.push_back(image2Msg->key_points); \
|
||||
localPoints3d.push_back(image1Msg->points); \
|
||||
localPoints3d.push_back(image2Msg->points); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors));
|
||||
|
||||
// 2 RGBD
|
||||
void CommonDataSubscriber::rgbd2Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2ScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2InfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd2OdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd2DataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd2OdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd2 callback");
|
||||
|
||||
rgbdSubs_.resize(2);
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,651 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
if(!image1Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
|
||||
if(!image2Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
|
||||
if(!image3Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
|
||||
localKeyPoints.push_back(image1Msg->key_points); \
|
||||
localKeyPoints.push_back(image2Msg->key_points); \
|
||||
localKeyPoints.push_back(image3Msg->key_points); \
|
||||
localPoints3d.push_back(image1Msg->points); \
|
||||
localPoints3d.push_back(image2Msg->points); \
|
||||
localPoints3d.push_back(image3Msg->points); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors));
|
||||
|
||||
// 3 RGBD
|
||||
void CommonDataSubscriber::rgbd3Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3ScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
|
||||
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3InfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd3OdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
|
||||
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd3DataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
|
||||
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd3OdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
|
||||
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
|
||||
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
|
||||
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
|
||||
localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDescriptor,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd3 callback");
|
||||
|
||||
rgbdSubs_.resize(3);
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,619 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
|
||||
rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
if(!image1Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
|
||||
if(!image2Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
|
||||
if(!image3Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
|
||||
if(!image4Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
|
||||
localKeyPoints.push_back(image1Msg->key_points); \
|
||||
localKeyPoints.push_back(image2Msg->key_points); \
|
||||
localKeyPoints.push_back(image3Msg->key_points); \
|
||||
localKeyPoints.push_back(image4Msg->key_points); \
|
||||
localPoints3d.push_back(image1Msg->points); \
|
||||
localPoints3d.push_back(image2Msg->points); \
|
||||
localPoints3d.push_back(image3Msg->points); \
|
||||
localPoints3d.push_back(image4Msg->points); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors));
|
||||
|
||||
// 4 RGBD
|
||||
void CommonDataSubscriber::rgbd4Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4Scan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4Scan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4ScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4InfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd4OdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd4DataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd4OdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd4 callback");
|
||||
|
||||
rgbdSubs_.resize(4);
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,371 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
|
||||
rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
|
||||
rtabmap_conversions::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
if(!image1Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
|
||||
if(!image2Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
|
||||
if(!image3Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
|
||||
if(!image4Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
|
||||
if(!image5Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
|
||||
localKeyPoints.push_back(image1Msg->key_points); \
|
||||
localKeyPoints.push_back(image2Msg->key_points); \
|
||||
localKeyPoints.push_back(image3Msg->key_points); \
|
||||
localKeyPoints.push_back(image4Msg->key_points); \
|
||||
localKeyPoints.push_back(image5Msg->key_points); \
|
||||
localPoints3d.push_back(image1Msg->points); \
|
||||
localPoints3d.push_back(image2Msg->points); \
|
||||
localPoints3d.push_back(image3Msg->points); \
|
||||
localPoints3d.push_back(image4Msg->points); \
|
||||
localPoints3d.push_back(image5Msg->points); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors));
|
||||
|
||||
// 5 RGBD
|
||||
void CommonDataSubscriber::rgbd5Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5Scan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5Scan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5ScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5InfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 5 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd5OdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5OdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5OdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5OdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd5OdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD5Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool /*subscribeUserData*/,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd5 callback");
|
||||
|
||||
rgbdSubs_.resize(5);
|
||||
for(int i=0; i<5; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,389 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
|
||||
rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
|
||||
rtabmap_conversions::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
|
||||
rtabmap_conversions::toCvShare(image6Msg, imageMsgs[5], depthMsgs[5]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(image6Msg->depth_camera_info); \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
if(!image1Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
|
||||
if(!image2Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
|
||||
if(!image3Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
|
||||
if(!image4Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
|
||||
if(!image5Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
|
||||
if(!image6Msg->global_descriptor.data.empty()) \
|
||||
globalDescriptorMsgs.push_back(image6Msg->global_descriptor); \
|
||||
localKeyPoints.push_back(image1Msg->key_points); \
|
||||
localKeyPoints.push_back(image2Msg->key_points); \
|
||||
localKeyPoints.push_back(image3Msg->key_points); \
|
||||
localKeyPoints.push_back(image4Msg->key_points); \
|
||||
localKeyPoints.push_back(image5Msg->key_points); \
|
||||
localKeyPoints.push_back(image6Msg->key_points); \
|
||||
localPoints3d.push_back(image1Msg->points); \
|
||||
localPoints3d.push_back(image2Msg->points); \
|
||||
localPoints3d.push_back(image3Msg->points); \
|
||||
localPoints3d.push_back(image4Msg->points); \
|
||||
localPoints3d.push_back(image5Msg->points); \
|
||||
localPoints3d.push_back(image6Msg->points); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors)); \
|
||||
localDescriptors.push_back(rtabmap::uncompressData(image6Msg->descriptors));
|
||||
|
||||
// 6 RGBD
|
||||
void CommonDataSubscriber::rgbd6Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6Scan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6Scan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6ScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6InfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 6 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd6OdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6OdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6OdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6OdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd6OdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5Msg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD6Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool /*subscribeUserData*/,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd6 callback");
|
||||
|
||||
rgbdSubs_.resize(6);
|
||||
for(int i=0; i<6; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,539 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imagesMsg->header.stamp);} \
|
||||
UASSERT(!imagesMsg->rgbd_images.empty()); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptorMsgs; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints; \
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d; \
|
||||
std::vector<cv::Mat> localDescriptors; \
|
||||
for(size_t i=0; i<imageMsgs.size(); ++i) \
|
||||
{ \
|
||||
rtabmap_conversions::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
|
||||
cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
|
||||
depthCameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].depth_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_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
// X RGBD + Odom
|
||||
void CommonDataSubscriber::rgbdXOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// X RGBD + User Data
|
||||
void CommonDataSubscriber::rgbdXDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXDataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// X RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbdXOdomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr imagesMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBDXCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback");
|
||||
|
||||
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_);
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync_, syncQueueSize_, rgbdXSub_, scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync_, syncQueueSize_, rgbdXSub_, scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync_, syncQueueSize_, rgbdXSub_, scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdXSub_.unsubscribe();
|
||||
rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
rgbdXSubOnly_->get_topic_name());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,497 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
void CommonDataSubscriber::scan2dCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scan3dCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scanDescCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
void CommonDataSubscriber::scan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::odomScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
void CommonDataSubscriber::dataScan2dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan3dCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScanDescCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan2dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan3dInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::odomDataScan2dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan3dCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScanDescCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan2dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan3dInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScanDescInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(scanMsg->header.stamp);}
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupScanCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool scan2dTopic,
|
||||
bool scanDescTopic,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
#else
|
||||
bool,
|
||||
#endif
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
|
||||
|
||||
if(subscribeOdom ||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
subscribeUserData ||
|
||||
#endif
|
||||
subscribeOdomInfo)
|
||||
{
|
||||
if(scanDescTopic)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
else
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(scanDescTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(scanDescTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(scanDescTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_);
|
||||
}
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(scanDescTopic)
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync_, syncQueueSize_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(scanDescTopic)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSubOnly_ = node.create_subscription<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
scanDescSubOnly_->get_topic_name());
|
||||
}
|
||||
else if(scan2dTopic)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
scan2dSubOnly_->get_topic_name());
|
||||
}
|
||||
else
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
scan3dSubOnly_->get_topic_name());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
// SensorData
|
||||
void CommonDataSubscriber::sensorDataCallback(
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::sensorDataInfoCallback(
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
|
||||
}
|
||||
// SensorData + Odom
|
||||
void CommonDataSubscriber::sensorDataOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::sensorDataOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr sensorDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(sensorDataMsg->header.stamp);}
|
||||
commonSensorDataCallback(sensorDataMsg, odomMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupSensorDataCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup SensorData callback");
|
||||
|
||||
sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
sensorDataSub_.unsubscribe();
|
||||
sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
node.get_name(),
|
||||
sensorDataSubOnly_->get_topic_name());
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,136 @@
|
||||
/*
|
||||
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_sync/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap_sync {
|
||||
|
||||
// Stereo
|
||||
void CommonDataSubscriber::stereoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::stereoInfoCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// Stereo + Odom
|
||||
void CommonDataSubscriber::stereoOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::stereoOdomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(leftImageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupStereoCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
|
||||
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace rtabmap_sync */
|
||||
@@ -0,0 +1,202 @@
|
||||
/*
|
||||
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_sync/rgb_sync.hpp"
|
||||
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/core/util2d.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_sync", options),
|
||||
compressedRate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
{
|
||||
int topicQueueSize = 10;
|
||||
int syncQueueSize = 10;
|
||||
bool approxSync = true;
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
|
||||
int queueSize = this->declare_parameter("queue_size", -1);
|
||||
if(queueSize != -1)
|
||||
{
|
||||
syncQueueSize = queueSize;
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
|
||||
"to \"sync_queue_size\" and will be removed "
|
||||
"in future versions! The value (%d) is copied to "
|
||||
"\"sync_queue_size\".", syncQueueSize);
|
||||
}
|
||||
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
|
||||
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
|
||||
|
||||
rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
|
||||
if(approxSyncMaxInterval > 0.0)
|
||||
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
|
||||
exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
image_transport::TransportHints hints(this);
|
||||
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
|
||||
|
||||
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
imageSub_.getSubscriber().getTopic().c_str(),
|
||||
cameraInfoSub_.getSubscriber()->get_topic_name());
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
|
||||
|
||||
syncDiagnostic_.reset(new SyncDiagnostic(this));
|
||||
syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
|
||||
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
|
||||
"can also help for better synchronization if framerates and/or delays are different. %s%s",
|
||||
this->get_name(),
|
||||
topicQueueSize,
|
||||
syncQueueSize,
|
||||
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()));
|
||||
}
|
||||
|
||||
|
||||
RGBSync::~RGBSync()
|
||||
{
|
||||
if(approxSync_)
|
||||
delete approxSync_;
|
||||
if(exactSync_)
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
void RGBSync::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image->header.stamp);
|
||||
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = cameraInfo->header.frame_id;
|
||||
msg.header.stamp = image->header.stamp;
|
||||
msg.rgb_camera_info = *cameraInfo;
|
||||
|
||||
if(rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
bool publishCompressed = true;
|
||||
if (compressedRate_ > 0.0)
|
||||
{
|
||||
if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
|
||||
{
|
||||
RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
|
||||
publishCompressed = false;
|
||||
}
|
||||
}
|
||||
|
||||
if(publishCompressed)
|
||||
{
|
||||
lastCompressedPublished_ = now();
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage msgCompressed = msg;
|
||||
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
|
||||
imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
|
||||
|
||||
rgbdImageCompressedPub_->publish(msgCompressed);
|
||||
}
|
||||
}
|
||||
|
||||
if(rgbdImagePub_->get_subscription_count())
|
||||
{
|
||||
msg.rgb = *image;
|
||||
rgbdImagePub_->publish(msg);
|
||||
}
|
||||
|
||||
if( stamp != rtabmap_conversions::timestampFromROS(image->header.stamp))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"%f->%f",
|
||||
stamp, rtabmap_conversions::timestampFromROS(image->header.stamp));
|
||||
}
|
||||
}
|
||||
syncDiagnostic_->tickOutput(image->header.stamp);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_sync::RGBSync)
|
||||
|
||||
@@ -0,0 +1,290 @@
|
||||
/*
|
||||
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_sync/rgbd_sync.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/core/util2d.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_sync", options),
|
||||
depthScale_(1.0),
|
||||
decimation_(1),
|
||||
compressedRate_(0),
|
||||
approxSyncMaxInterval_(0.0),
|
||||
approxSyncDepth_(0),
|
||||
exactSyncDepth_(0)
|
||||
{
|
||||
int topicQueueSize = 10;
|
||||
int syncQueueSize = 10;
|
||||
bool approxSync = true;
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_);
|
||||
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
|
||||
int queueSize = this->declare_parameter("queue_size", -1);
|
||||
if(queueSize != -1)
|
||||
{
|
||||
syncQueueSize = queueSize;
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
|
||||
"to \"sync_queue_size\" and will be removed "
|
||||
"in future versions! The value (%d) is copied to "
|
||||
"\"sync_queue_size\".", syncQueueSize);
|
||||
}
|
||||
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
|
||||
depthScale_ = this->declare_parameter("depth_scale", depthScale_);
|
||||
decimation_ = this->declare_parameter("decimation", decimation_);
|
||||
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
|
||||
|
||||
if(decimation_<1)
|
||||
{
|
||||
decimation_ = 1;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
|
||||
|
||||
rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
if(approxSyncMaxInterval_ > 0.0)
|
||||
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
|
||||
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
}
|
||||
|
||||
std::string rgbImageTransport = this->declare_parameter<std::string>("rgb_image_transport", "raw");
|
||||
std::string depthImageTransport = this->declare_parameter<std::string>("depth_image_transport", "raw");
|
||||
std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
|
||||
std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
|
||||
imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
|
||||
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
|
||||
imageSub_.getSubscriber().getTopic().c_str(),
|
||||
imageDepthSub_.getSubscriber().getTopic().c_str(),
|
||||
cameraInfoSub_.getSubscriber()->get_topic_name());
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
|
||||
|
||||
syncDiagnostic_.reset(new SyncDiagnostic(this));
|
||||
syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
|
||||
uFormat("%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. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
|
||||
"can also help for better synchronization if framerates and/or delays are different. %s%s",
|
||||
get_name(),
|
||||
topicQueueSize,
|
||||
syncQueueSize,
|
||||
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()));
|
||||
}
|
||||
|
||||
RGBDSync::~RGBDSync()
|
||||
{
|
||||
delete approxSyncDepth_;
|
||||
delete exactSyncDepth_;
|
||||
}
|
||||
|
||||
void RGBDSync::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image->header.stamp);
|
||||
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
|
||||
double depthStamp = rtabmap_conversions::timestampFromROS(depth->header.stamp);
|
||||
|
||||
double stampDiff = fabs(rgbStamp - depthStamp);
|
||||
if(stampDiff > 0.010 && approxSyncMaxInterval_ == 0.0)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
|
||||
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
|
||||
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
|
||||
"approx_sync=false if streams have all the exact same timestamp. Setting approx_sync_max_interval "
|
||||
"will suppress this warning.",
|
||||
stampDiff,
|
||||
rgbStamp,
|
||||
depthStamp);
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage::UniquePtr msg(new rtabmap_msgs::msg::RGBDImage);
|
||||
msg->header.frame_id = cameraInfo->header.frame_id;
|
||||
msg->header.stamp = rgbStamp>depthStamp?image->header.stamp:depth->header.stamp;
|
||||
if(decimation_>1 && !(depth->width % decimation_ == 0 && depth->height % decimation_ == 0))
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
|
||||
"Images won't be resized.", decimation_, depth->width, depth->height);
|
||||
decimation_ = 1;
|
||||
}
|
||||
if(decimation_>1)
|
||||
{
|
||||
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
rtabmap_conversions::cameraModelToROS(model.scaled(1.0f/float(decimation_)), info);
|
||||
info.header = cameraInfo->header;
|
||||
msg->rgb_camera_info = info;
|
||||
msg->depth_camera_info = info;
|
||||
}
|
||||
else
|
||||
{
|
||||
msg->rgb_camera_info = *cameraInfo;
|
||||
msg->depth_camera_info = *cameraInfo;
|
||||
}
|
||||
|
||||
cv::Mat rgbMat;
|
||||
cv::Mat depthMat;
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
|
||||
rgbMat = imagePtr->image;
|
||||
depthMat = imageDepthPtr->image;
|
||||
|
||||
if(decimation_>1)
|
||||
{
|
||||
rgbMat = rtabmap::util2d::decimate(rgbMat, decimation_);
|
||||
depthMat = rtabmap::util2d::decimate(depthMat, decimation_);
|
||||
}
|
||||
|
||||
if(depthScale_ != 1.0)
|
||||
{
|
||||
depthMat*=depthScale_;
|
||||
}
|
||||
|
||||
if(rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
bool publishCompressed = true;
|
||||
if (compressedRate_ > 0.0)
|
||||
{
|
||||
if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
|
||||
{
|
||||
RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
|
||||
publishCompressed = false;
|
||||
}
|
||||
}
|
||||
|
||||
if(publishCompressed)
|
||||
{
|
||||
lastCompressedPublished_ = now();
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_msgs::msg::RGBDImage);
|
||||
msgCompressed->header = msg->header;
|
||||
msgCompressed->rgb_camera_info = msg->rgb_camera_info;
|
||||
msgCompressed->depth_camera_info = msg->depth_camera_info;
|
||||
|
||||
cv_bridge::CvImage cvImg;
|
||||
cvImg.header = image->header;
|
||||
cvImg.image = rgbMat;
|
||||
cvImg.encoding = image->encoding;
|
||||
cvImg.toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);
|
||||
|
||||
msgCompressed->depth_compressed.header = imageDepthPtr->header;
|
||||
msgCompressed->depth_compressed.data = rtabmap::compressImage(depthMat, ".png");
|
||||
|
||||
msgCompressed->depth_compressed.format = "png";
|
||||
|
||||
rgbdImageCompressedPub_->publish(std::move(msgCompressed));
|
||||
}
|
||||
}
|
||||
|
||||
if(rgbdImagePub_->get_subscription_count())
|
||||
{
|
||||
cv_bridge::CvImage cvImg;
|
||||
cvImg.header = image->header;
|
||||
cvImg.image = rgbMat;
|
||||
cvImg.encoding = image->encoding;
|
||||
cvImg.toImageMsg(msg->rgb);
|
||||
|
||||
cv_bridge::CvImage cvDepth;
|
||||
cvDepth.header = depth->header;
|
||||
cvDepth.image = depthMat;
|
||||
cvDepth.encoding = depth->encoding;
|
||||
cvDepth.toImageMsg(msg->depth);
|
||||
|
||||
rgbdImagePub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
if( rgbStamp != rtabmap_conversions::timestampFromROS(image->header.stamp) ||
|
||||
depthStamp != rtabmap_conversions::timestampFromROS(depth->header.stamp))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"rgb=%f->%f depth=%f->%f",
|
||||
rgbStamp, rtabmap_conversions::timestampFromROS(image->header.stamp),
|
||||
depthStamp, rtabmap_conversions::timestampFromROS(depth->header.stamp));
|
||||
}
|
||||
}
|
||||
syncDiagnostic_->tickOutput(image->header.stamp);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_sync::RGBDSync)
|
||||
@@ -0,0 +1,322 @@
|
||||
/*
|
||||
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_sync/rgbdx_sync.hpp>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
|
||||
Node("rgbd_sync", options),
|
||||
SYNC_INIT(rgbd2),
|
||||
SYNC_INIT(rgbd3),
|
||||
SYNC_INIT(rgbd4),
|
||||
SYNC_INIT(rgbd5),
|
||||
SYNC_INIT(rgbd6),
|
||||
SYNC_INIT(rgbd7),
|
||||
SYNC_INIT(rgbd8)
|
||||
{
|
||||
int topicQueueSize = 10;
|
||||
int syncQueueSize = 10;
|
||||
bool approxSync = true;
|
||||
int rgbdCameras = 2;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
|
||||
int queueSize = this->declare_parameter("queue_size", -1);
|
||||
if(queueSize != -1)
|
||||
{
|
||||
syncQueueSize = queueSize;
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
|
||||
"to \"sync_queue_size\" and will be removed "
|
||||
"in future versions! The value (%d) is copied to "
|
||||
"\"sync_queue_size\".", syncQueueSize);
|
||||
}
|
||||
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
|
||||
if(approxSync)
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
|
||||
|
||||
rgbdImagesPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
UASSERT(rgbdCameras>=2 && rgbdCameras<=8);
|
||||
|
||||
rgbdSubs_.resize(rgbdCameras);
|
||||
for(int i=0; i<rgbdCameras; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
}
|
||||
|
||||
std::string name_ = get_name();
|
||||
std::string subscribedTopicsMsg_;
|
||||
if(rgbdCameras==2)
|
||||
{
|
||||
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd2ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==3)
|
||||
{
|
||||
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd3ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==4)
|
||||
{
|
||||
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd4ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==5)
|
||||
{
|
||||
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd5ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==6)
|
||||
{
|
||||
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd6ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==7)
|
||||
{
|
||||
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd7ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
else if(rgbdCameras==8)
|
||||
{
|
||||
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
|
||||
if(approxSync && approxSyncMaxInterval>0.0)
|
||||
{
|
||||
rgbd8ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
}
|
||||
}
|
||||
|
||||
std::string subscribedTopicsMsg = uFormat("%s%s", subscribedTopicsMsg_.c_str(),
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
|
||||
RCLCPP_INFO(this->get_logger(), subscribedTopicsMsg.c_str());
|
||||
|
||||
// Setup diagnostic
|
||||
syncDiagnostic_.reset(new SyncDiagnostic(this));
|
||||
syncDiagnostic_->init("",
|
||||
uFormat("%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. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
|
||||
"can also help for better synchronization if framerates and/or delays are different.%s%s",
|
||||
get_name(),
|
||||
topicQueueSize,
|
||||
syncQueueSize,
|
||||
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()));
|
||||
}
|
||||
|
||||
RGBDXSync::~RGBDXSync()
|
||||
{
|
||||
SYNC_DEL(rgbd2);
|
||||
SYNC_DEL(rgbd3);
|
||||
SYNC_DEL(rgbd4);
|
||||
SYNC_DEL(rgbd5);
|
||||
SYNC_DEL(rgbd6);
|
||||
SYNC_DEL(rgbd7);
|
||||
SYNC_DEL(rgbd8);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd2Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::RGBDImages output;
|
||||
output.header = image0->header;
|
||||
output.rgbd_images.resize(2);
|
||||
output.rgbd_images[0]=(*image0);
|
||||
output.rgbd_images[1]=(*image1);
|
||||
rgbdImagesPub_->publish(output);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd3Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd4Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd5Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd6Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd7Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
void RGBDXSync::rgbd8Callback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
|
||||
{
|
||||
syncDiagnostic_->tickInput(image0->header.stamp);
|
||||
rtabmap_msgs::msg::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);
|
||||
syncDiagnostic_->tickOutput(image0->header.stamp);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_sync::RGBDXSync)
|
||||
@@ -0,0 +1,223 @@
|
||||
/*
|
||||
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_sync/stereo_sync.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
|
||||
namespace rtabmap_sync
|
||||
{
|
||||
|
||||
StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
|
||||
Node("stereo_sync", options),
|
||||
compressedRate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
{
|
||||
int topicQueueSize = 10;
|
||||
int syncQueueSize = 10;
|
||||
bool approxSync = false;
|
||||
double approxSyncMaxInterval = 0.0;
|
||||
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
approxSync = this->declare_parameter("approx_sync", approxSync);
|
||||
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
|
||||
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
|
||||
int queueSize = this->declare_parameter("queue_size", -1);
|
||||
if(queueSize != -1)
|
||||
{
|
||||
syncQueueSize = queueSize;
|
||||
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
|
||||
"to \"sync_queue_size\" and will be removed "
|
||||
"in future versions! The value (%d) is copied to "
|
||||
"\"sync_queue_size\".", syncQueueSize);
|
||||
}
|
||||
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
|
||||
qos = this->declare_parameter("qos", qos);
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
|
||||
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
|
||||
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
|
||||
|
||||
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
rgbdImageCompressedPub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
|
||||
if(approxSyncMaxInterval>0.0)
|
||||
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
|
||||
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
|
||||
exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
|
||||
}
|
||||
|
||||
image_transport::TransportHints hints(this);
|
||||
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
|
||||
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
|
||||
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
imageLeftSub_.getSubscriber().getTopic().c_str(),
|
||||
imageRightSub_.getSubscriber().getTopic().c_str(),
|
||||
cameraInfoLeftSub_.getSubscriber()->get_topic_name(),
|
||||
cameraInfoRightSub_.getSubscriber()->get_topic_name());
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
|
||||
|
||||
syncDiagnostic_.reset(new SyncDiagnostic(this));
|
||||
syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(),
|
||||
uFormat("%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. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
|
||||
"can also help for better synchronization if framerates and/or delays are different.%s%s",
|
||||
get_name(),
|
||||
topicQueueSize,
|
||||
syncQueueSize,
|
||||
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()));
|
||||
}
|
||||
|
||||
StereoSync::~StereoSync()
|
||||
{
|
||||
delete approxSync_;
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
void StereoSync::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
|
||||
{
|
||||
syncDiagnostic_->tickInput(imageLeft->header.stamp);
|
||||
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);
|
||||
double rightStamp = rtabmap_conversions::timestampFromROS(imageRight->header.stamp);
|
||||
|
||||
double stampDiff = fabs(leftStamp - rightStamp);
|
||||
if(stampDiff > 0.010)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is "
|
||||
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
|
||||
"synchronized, use approx_sync:=false. Otherwise, you may want "
|
||||
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
|
||||
stampDiff,
|
||||
leftStamp,
|
||||
rightStamp);
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage::UniquePtr msg(new rtabmap_msgs::msg::RGBDImage);
|
||||
msg->header.frame_id = cameraInfoLeft->header.frame_id;
|
||||
msg->header.stamp = leftStamp>rightStamp?imageLeft->header.stamp:imageRight->header.stamp;
|
||||
msg->rgb_camera_info = *cameraInfoLeft;
|
||||
msg->depth_camera_info = *cameraInfoRight;
|
||||
if(rgbdImageCompressedPub_->get_subscription_count())
|
||||
{
|
||||
bool publishCompressed = true;
|
||||
if (compressedRate_ > 0.0)
|
||||
{
|
||||
if ( lastCompressedPublished_ + rclcpp::Duration::from_seconds(1.0/compressedRate_) > now())
|
||||
{
|
||||
RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
|
||||
publishCompressed = false;
|
||||
}
|
||||
}
|
||||
|
||||
if(publishCompressed)
|
||||
{
|
||||
lastCompressedPublished_ = now();
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_msgs::msg::RGBDImage);
|
||||
*msgCompressed = *msg;
|
||||
|
||||
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
|
||||
imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);
|
||||
|
||||
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
|
||||
imageDepthPtr->toCompressedImageMsg(msgCompressed->depth_compressed, cv_bridge::JPG);
|
||||
|
||||
rgbdImageCompressedPub_->publish(std::move(msgCompressed));
|
||||
}
|
||||
}
|
||||
|
||||
if(rgbdImagePub_->get_subscription_count())
|
||||
{
|
||||
msg->rgb = *imageLeft;
|
||||
msg->depth = *imageRight;
|
||||
rgbdImagePub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
if( leftStamp != rtabmap_conversions::timestampFromROS(imageLeft->header.stamp) ||
|
||||
rightStamp != rtabmap_conversions::timestampFromROS(imageRight->header.stamp))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
|
||||
"sure the node publishing the topics doesn't override the same data after publishing them. A "
|
||||
"solution is to use this node within another nodelet manager. Stamps: "
|
||||
"left%f->%f right=%f->%f",
|
||||
leftStamp, rtabmap_conversions::timestampFromROS(imageLeft->header.stamp),
|
||||
rightStamp, rtabmap_conversions::timestampFromROS(imageRight->header.stamp));
|
||||
}
|
||||
}
|
||||
syncDiagnostic_->tickOutput(imageLeft->header.stamp);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_sync::StereoSync)
|
||||
|
||||
Reference in New Issue
Block a user