feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -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 */
|
||||
Reference in New Issue
Block a user