m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(slam): add rtabmap_ros

Browse Source
This commit is contained in:
X-lanni
2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_ros/rtabmap_util/src/DbPlayerNode.cpp
+686
View File
@@ -0,0 +1,686 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <rosgraph_msgs/Clock.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/Odometry.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <image_transport/image_transport.h>
#include <tf2_ros/transform_broadcaster.h>
#include <std_srvs/Empty.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap_msgs/SetGoal.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
#include <cmath>
#ifndef _WIN32
#include <sys/ioctl.h>
#include <termios.h>
bool spacehit()
{
bool charAvailable = true;
bool hit = false;
while(charAvailable)
{
termios term;
tcgetattr(0, &term);
termios term2 = term;
term2.c_lflag &= ~ICANON;
term2.c_lflag &= ~ECHO;
term2.c_lflag &= ~ISIG;
term2.c_cc[VMIN] = 0;
term2.c_cc[VTIME] = 0;
tcsetattr(0, TCSANOW, &term2);
int c = getchar();
if(c != EOF)
{
if(c == ' ')
{
hit = true;
}
}
else
{
charAvailable = false;
}
tcsetattr(0, TCSANOW, &term);
}
return hit;
}
#endif
bool paused = false;
bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused)
{
ROS_WARN("Already paused!");
}
else
{
paused = true;
ROS_INFO("paused!");
}
return true;
}
bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused)
{
ROS_WARN("Already running!");
}
else
{
paused = false;
ROS_INFO("resumed!");
}
return true;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "data_player");
//ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug);
//ULogger::setEventLevel(ULogger::kWarning);
bool publishClock = false;
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--clock") == 0)
{
publishClock = true;
}
}
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
std::string frameId = "base_link";
std::string odomFrameId = "odom";
std::string cameraFrameId = "camera_optical_link";
std::string scanFrameId = "base_laser_link";
double rate = 1.0f;
std::string databasePath = "";
bool publishTf = true;
int startId = 0;
bool useDbStamps = true;
pnh.param("frame_id", frameId, frameId);
pnh.param("odom_frame_id", odomFrameId, odomFrameId);
pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
pnh.param("scan_frame_id", scanFrameId, scanFrameId);
pnh.param("rate", rate, rate); // Ratio of the database stamps
pnh.param("database", databasePath, databasePath);
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("start_id", startId, startId);
// A general 360 lidar with 0.5 deg increment
double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
pnh.param<double>("scan_range_max", scanRangeMax, 60);
ROS_INFO("frame_id = %s", frameId.c_str());
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
ROS_INFO("rate = %f", rate);
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
ROS_INFO("start_id = %d", startId);
ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
if(databasePath.empty())
{
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
return -1;
}
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/')
{
databasePath = UDirectory::currentDir(true) + databasePath;
}
ROS_INFO("database = %s", databasePath.c_str());
rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
if(!reader.init())
{
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
return -1;
}
ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
image_transport::ImageTransport it(nh);
image_transport::Publisher imagePub;
image_transport::Publisher rgbPub;
image_transport::Publisher depthPub;
image_transport::Publisher leftPub;
image_transport::Publisher rightPub;
ros::Publisher rgbCamInfoPub;
ros::Publisher depthCamInfoPub;
ros::Publisher leftCamInfoPub;
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
ros::Publisher scanCloudPub;
ros::Publisher globalPosePub;
ros::Publisher gpsFixPub;
ros::Publisher clockPub;
tf2_ros::TransformBroadcaster tfBroadcaster;
if(publishClock)
{
clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
}
UTimer timer;
rtabmap::CameraInfo cameraInfo;
rtabmap::SensorData data = reader.takeImage(&cameraInfo);
rtabmap::OdometryInfo odomInfo;
odomInfo.reg.covariance = cameraInfo.odomCovariance;
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id())
{
ROS_INFO("Reading sensor data %d...", odom.data().id());
ros::Time time(odom.data().stamp());
if(publishClock)
{
rosgraph_msgs::Clock msg;
msg.clock = time;
clockPub.publish(msg);
}
sensor_msgs::CameraInfo camInfoA; //rgb or left
sensor_msgs::CameraInfo camInfoB; //depth or right
camInfoA.K.assign(0);
camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
camInfoA.R.assign(0);
camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
camInfoA.P.assign(0);
camInfoA.P[10] = 1;
camInfoA.header.frame_id = cameraFrameId;
camInfoA.header.stamp = time;
camInfoB = camInfoA;
int type = -1;
if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
{
if(odom.data().cameraModels().size() > 1)
{
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//depth
if(odom.data().cameraModels().size())
{
camInfoA.D.resize(5,0);
camInfoA.P[0] = odom.data().cameraModels()[0].fx();
camInfoA.K[0] = odom.data().cameraModels()[0].fx();
camInfoA.P[5] = odom.data().cameraModels()[0].fy();
camInfoA.K[4] = odom.data().cameraModels()[0].fy();
camInfoA.P[2] = odom.data().cameraModels()[0].cx();
camInfoA.K[2] = odom.data().cameraModels()[0].cx();
camInfoA.P[6] = odom.data().cameraModels()[0].cy();
camInfoA.K[5] = odom.data().cameraModels()[0].cy();
camInfoB = camInfoA;
}
type=0;
if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
}
}
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{
if(odom.data().stereoCameraModels().size() > 1)
{
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//stereo
if(odom.data().stereoCameraModels()[0].isValidForProjection())
{
camInfoA.D.resize(8,0);
camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
}
type=1;
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
}
}
else
{
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
}
camInfoA.height = odom.data().imageRaw().rows;
camInfoA.width = odom.data().imageRaw().cols;
camInfoB.height = odom.data().depthOrRightRaw().rows;
camInfoB.width = odom.data().depthOrRightRaw().cols;
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
{
scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
ROS_INFO("Scan will be published.");
}
else
{
ROS_INFO("Scan will be published with those parameters:");
ROS_INFO(" scan_angle_min=%f", scanAngleMin);
ROS_INFO(" scan_angle_max=%f", scanAngleMax);
ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
ROS_INFO(" scan_range_min=%f", scanRangeMin);
ROS_INFO(" scan_range_max=%f", scanRangeMax);
}
}
else if(scanCloudPub.getTopic().empty())
{
scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
ROS_INFO("Scan cloud will be published.");
}
}
if(!odom.data().globalPose().isNull() &&
odom.data().globalPoseCovariance().cols==6 &&
odom.data().globalPoseCovariance().rows==6)
{
if(globalPosePub.getTopic().empty())
{
globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
ROS_INFO("Global pose will be published.");
}
}
if(odom.data().gps().stamp() > 0.0)
{
if(gpsFixPub.getTopic().empty())
{
gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
ROS_INFO("GPS will be published.");
}
}
// publish transforms first
if(publishTf)
{
rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModels().size() == 1)
{
localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
}
if(!localTransform.isNull())
{
geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera);
}
if(!odom.pose().isNull())
{
geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase);
}
if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
{
geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId;
baseToLaserScan.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan);
}
}
if(!odom.pose().isNull())
{
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
if(odometryPub.getNumSubscribers())
{
nav_msgs::Odometry odomMsg;
odomMsg.child_frame_id = frameId;
odomMsg.header.frame_id = odomFrameId;
odomMsg.header.stamp = time;
rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
UASSERT(odomMsg.pose.covariance.size() == 36 &&
odom.covariance().total() == 36 &&
odom.covariance().type() == CV_64FC1);
memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
odometryPub.publish(odomMsg);
}
}
// Publish async topics first (so that they can catched by rtabmap before the image topics)
if(globalPosePub.getNumSubscribers() > 0 &&
!odom.data().globalPose().isNull() &&
odom.data().globalPoseCovariance().cols==6 &&
odom.data().globalPoseCovariance().rows==6)
{
geometry_msgs::PoseWithCovarianceStamped msg;
rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
msg.header.frame_id = frameId;
msg.header.stamp = time;
globalPosePub.publish(msg);
}
if(odom.data().gps().stamp() > 0.0)
{
sensor_msgs::NavSatFix msg;
msg.longitude = odom.data().gps().longitude();
msg.latitude = odom.data().gps().latitude();
msg.altitude = odom.data().gps().altitude();
msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
msg.header.frame_id = frameId;
msg.header.stamp.fromSec(odom.data().gps().stamp());
gpsFixPub.publish(msg);
}
if(type >= 0)
{
if(rgbCamInfoPub.getNumSubscribers() && type == 0)
{
rgbCamInfoPub.publish(camInfoA);
}
if(leftCamInfoPub.getNumSubscribers() && type == 1)
{
leftCamInfoPub.publish(camInfoA);
}
if(depthCamInfoPub.getNumSubscribers() && type == 0)
{
depthCamInfoPub.publish(camInfoB);
}
if(rightCamInfoPub.getNumSubscribers() && type == 1)
{
rightCamInfoPub.publish(camInfoB);
}
}
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
{
cv_bridge::CvImage img;
if(odom.data().imageRaw().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = odom.data().imageRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
if(imagePub.getNumSubscribers())
{
imagePub.publish(imageRosMsg);
}
if(rgbPub.getNumSubscribers() && type == 0)
{
rgbPub.publish(imageRosMsg);
}
if(leftPub.getNumSubscribers() && type == 1)
{
leftPub.publish(imageRosMsg);
leftCamInfoPub.publish(camInfoA);
}
}
if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
{
cv_bridge::CvImage img;
if(odom.data().depthRaw().type() == CV_32FC1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
else
{
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
img.image = odom.data().depthRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
depthPub.publish(imageRosMsg);
depthCamInfoPub.publish(camInfoB);
}
if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::MONO8;
img.image = odom.data().rightRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
rightPub.publish(imageRosMsg);
rightCamInfoPub.publish(camInfoB);
}
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::LaserScan msg;
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
msg.angle_min = scanAngleMin;
msg.angle_max = scanAngleMax;
msg.angle_increment = scanAngleIncrement;
msg.time_increment = 0.0;
msg.scan_time = 0;
msg.range_min = scanRangeMin;
msg.range_max = scanRangeMax;
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
msg.angle_min = odom.data().laserScanRaw().angleMin();
msg.angle_max = odom.data().laserScanRaw().angleMax();
msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
msg.range_min = odom.data().laserScanRaw().rangeMin();
msg.range_max = odom.data().laserScanRaw().rangeMax();
}
uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
msg.ranges.assign(rangesSize, 0.0);
const cv::Mat & scan = odom.data().laserScanRaw().data();
for (int i=0; i<scan.cols; ++i)
{
const float * ptr = scan.ptr<float>(0,i);
double range = hypot(ptr[0], ptr[1]);
if (range >= msg.range_min && range <=msg.range_max)
{
double angle = atan2(ptr[1], ptr[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
{
int index = (angle - msg.angle_min) / msg.angle_increment;
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
{
msg.ranges[index] = range;
}
}
}
}
scanPub.publish(msg);
}
else if(scanCloudPub.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
scanCloudPub.publish(msg);
}
}
if(odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = (const char *)odom.data().userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
int goalId = atoi(strs.rbegin()->c_str());
if(goalId > 0)
{
ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
rtabmap_msgs::SetGoal setGoalSrv;
setGoalSrv.request.node_id = goalId;
setGoalSrv.request.node_label = "";
if(!ros::service::call("set_goal", setGoalSrv))
{
ROS_ERROR("Can't call \"set_goal\" service");
}
}
}
}
}
ros::spinOnce();
while(ros::ok())
{
#ifndef _WIN32
if (spacehit()) {
paused = !paused;
if(paused)
{
ROS_INFO("paused!");
}
else
{
ROS_INFO("resumed!");
}
}
#endif
if(!paused)
{
break;
}
uSleep(100);
ros::spinOnce();
}
timer.restart();
cameraInfo = rtabmap::CameraInfo();
data = reader.takeImage(&cameraInfo);
odomInfo.reg.covariance = cameraInfo.odomCovariance;
odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
acquisitionTime = timer.ticks();
}
return 0;
}
rtabmap_ros/rtabmap_util/src/DisparityToDepthNode.cpp
+39
View File
@@ -0,0 +1,39 @@
/*
Copyright (c) 2010-2019, 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_util/disparity_to_depth.hpp"
#include "rtabmap/utilite/ULogger.h"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/ImuToTFNode.cpp
+39
View File
@@ -0,0 +1,39 @@
/*
Copyright (c) 2010-2019, 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_util/imu_to_tf.hpp"
#include "rtabmap/utilite/ULogger.h"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/LidarDeskewingNode.cpp
+39
View File
@@ -0,0 +1,39 @@
/*
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_util/lidar_deskewing.hpp"
#include "rtabmap/utilite/ULogger.h"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/MapAssemblerNode.cpp
+88
View File
@@ -0,0 +1,88 @@
/*
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_util/map_assembler.hpp"
#include <rtabmap/utilite/UStl.h>
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
// process "--params" argument
std::vector<std::string> arguments;
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parameters;
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
UWARN("Node will now exit after showing default parameters because "
"argument \"--params\" is detected!");
exit(0);
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
else if(strcmp(argv[i], "--uwarn") == 0)
{
ULogger::setLevel(ULogger::kWarning);
}
arguments.push_back(argv[i]);
}
rclcpp::NodeOptions options;
options.arguments(arguments);
rclcpp::init(argc, argv);
auto node = std::make_shared<rtabmap_util::MapAssembler>(options);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/MapOptimizerNode.cpp
+345
View File
@@ -0,0 +1,345 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include "rtabmap_msgs/MapData.h"
#include "rtabmap_msgs/MapGraph.h"
#include "rtabmap_conversions/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <tf2_ros/transform_broadcaster.h>
#include <boost/thread.hpp>
using namespace rtabmap;
class MapOptimizer
{
public:
MapOptimizer() :
mapFrameId_("map"),
odomFrameId_("odom"),
globalOptimization_(true),
optimizeFromLastNode_(false),
mapToOdom_(rtabmap::Transform::getIdentity()),
transformThread_(0)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
double epsilon = 0.0;
bool robust = true;
bool slam2d =false;
int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
int iterations = 100;
bool ignoreVariance = false;
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("iterations", iterations, iterations);
pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
pnh.param("global_optimization", globalOptimization_, globalOptimization_);
pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
pnh.param("epsilon", epsilon, epsilon);
pnh.param("robust", robust, robust);
pnh.param("slam_2d", slam2d, slam2d);
pnh.param("strategy", strategy, strategy);
UASSERT(iterations > 0);
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), uBool2Str(slam2d)));
parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
optimizer_ = Optimizer::create(parameters);
double tfDelay = 0.05; // 20 Hz
bool publishTf = true;
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay);
mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
mapDataPub_ = nh.advertise<rtabmap_msgs::MapData>(nh.resolveName("mapData")+"_optimized", 1);
mapGraphPub_ = nh.advertise<rtabmap_msgs::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
if(publishTf)
{
ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
}
}
~MapOptimizer()
{
if(transformThread_)
{
transformThread_->join();
delete transformThread_;
}
}
void publishLoop(double tfDelay)
{
if(tfDelay == 0)
return;
ros::Rate r(1.0 / tfDelay);
while(ros::ok())
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
geometry_msgs::TransformStamped msg;
msg.child_frame_id = odomFrameId_;
msg.header.frame_id = mapFrameId_;
msg.header.stamp = tfExpiration;
rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
tfBroadcaster_.sendTransform(msg);
mapToOdomMutex_.unlock();
r.sleep();
}
}
void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
{
// save new poses and constraints
// Assuming that nodes/constraints are all linked together
UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
bool dataChanged = false;
std::multimap<int, Link> newConstraints;
for(unsigned int i=0; i<msg->graph.links.size(); ++i)
{
Link link = rtabmap_conversions::linkFromROS(msg->graph.links[i]);
newConstraints.insert(std::make_pair(link.from(), link));
bool edgeAlreadyAdded = false;
for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
iter != cachedConstraints_.end() && iter->first == link.from();
++iter)
{
if(iter->second.to() == link.to())
{
edgeAlreadyAdded = true;
if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
{
ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
link.transform().prettyPrint().c_str());
dataChanged = true;
}
}
}
if(!edgeAlreadyAdded)
{
cachedConstraints_.insert(std::make_pair(link.from(), link));
}
}
std::map<int, Signature> newNodeInfos;
// add new odometry poses
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
int id = msg->nodes[i].id;
Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->nodes[i].pose);
Signature s = rtabmap_conversions::nodeInfoFromROS(msg->nodes[i]);
newNodeInfos.insert(std::make_pair(id, s));
std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
{
dataChanged = true;
}
}
if(dataChanged)
{
ROS_WARN("Graph data has changed! Reset cache...");
cachedConstraints_ = newConstraints;
cachedNodeInfos_ = newNodeInfos;
}
//match poses in the graph
std::multimap<int, Link> constraints;
std::map<int, Signature> nodeInfos;
if(globalOptimization_)
{
constraints = cachedConstraints_;
nodeInfos = cachedNodeInfos_;
}
else
{
constraints = newConstraints;
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
{
std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
if(iter != cachedNodeInfos_.end())
{
nodeInfos.insert(*iter);
}
else
{
ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
return;
}
}
}
std::map<int, Transform> poses;
for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
{
poses.insert(std::make_pair(iter->first, iter->second.getPose()));
}
// Optimize only if there is a subscriber
if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
Transform mapCorrection = Transform::getIdentity();
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
if(poses.size() > 1 && constraints.size() > 0)
{
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
optimizer_->getConnectedGraph(
fromId,
poses,
constraints,
posesOut,
linksOut);
optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
mapToOdom_ = mapCorrection;
mapToOdomMutex_.unlock();
}
else if(poses.size() == 1 && constraints.size() == 0)
{
optimizedPoses = poses;
}
else if(poses.size() == 0 && constraints.size())
{
ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must "
"not be null if there are edges.",
(int)poses.size(), (int)constraints.size());
}
rtabmap_msgs::MapData outputDataMsg;
rtabmap_msgs::MapGraph outputGraphMsg;
rtabmap_conversions::mapGraphToROS(optimizedPoses,
linksOut,
mapCorrection,
outputGraphMsg);
if(mapGraphPub_.getNumSubscribers())
{
outputGraphMsg.header = msg->header;
mapGraphPub_.publish(outputGraphMsg);
}
if(mapDataPub_.getNumSubscribers())
{
outputDataMsg.header = msg->header;
outputDataMsg.graph = outputGraphMsg;
outputDataMsg.nodes = msg->nodes;
if(posesOut.size() > msg->nodes.size())
{
std::set<int> addedNodes;
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
addedNodes.insert(msg->nodes[i].id);
}
std::list<int> toAdd;
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
if(addedNodes.find(iter->first) == addedNodes.end())
{
toAdd.push_back(iter->first);
}
}
if(toAdd.size())
{
int oi = outputDataMsg.nodes.size();
outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
{
UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
rtabmap_conversions::nodeToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
++oi;
}
}
}
mapDataPub_.publish(outputDataMsg);
}
ROS_INFO("Time graph optimization = %f s", timer.ticks());
}
}
private:
std::string mapFrameId_;
std::string odomFrameId_;
bool globalOptimization_;
bool optimizeFromLastNode_;
Optimizer * optimizer_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
ros::Subscriber mapDataTopic_;
ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_;
std::multimap<int, Link> cachedConstraints_;
std::map<int, Signature> cachedNodeInfos_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_optimizer");
MapOptimizer optimizer;
ros::spin();
return 0;
}
rtabmap_ros/rtabmap_util/src/MapsManager.cpp
+1413
View File
File diff suppressed because it is too large Load Diff
rtabmap_ros/rtabmap_util/src/ObstaclesDetectionNode.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_util/obstacles_detection.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/OdomMsgToTFNode.cpp
+92
View File
@@ -0,0 +1,92 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_broadcaster.h>
#include <rtabmap_conversions/MsgConversion.h>
class OdomMsgToTF
{
public:
OdomMsgToTF() :
frameId_(""),
odomFrameId_("")
{
ros::NodeHandle pnh("~");
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
ros::NodeHandle nh;
odomTopic_ = nh.subscribe("odom", 1, &OdomMsgToTF::odomReceivedCallback, this);
}
virtual ~OdomMsgToTF(){}
void odomReceivedCallback(const nav_msgs::OdometryConstPtr & msg)
{
if(frameId_.empty())
{
frameId_ = msg->child_frame_id;
}
if(odomFrameId_.empty())
{
odomFrameId_ = msg->header.frame_id;
}
geometry_msgs::TransformStamped t;
rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
if(pose.isNull())
{
ROS_WARN("Odometry received is null! Cannot send tf...");
}
else
{
t.child_frame_id = frameId_;
t.header.frame_id = odomFrameId_;
t.header.stamp = msg->header.stamp;
rtabmap_conversions::transformToGeometryMsg(pose, t.transform);
tfBroadcaster_.sendTransform(t);
}
}
private:
std::string frameId_;
std::string odomFrameId_;
ros::Subscriber odomTopic_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "odom_msg_to_tf");
OdomMsgToTF odomToTf;
ros::spin();
return 0;
}
rtabmap_ros/rtabmap_util/src/PointCloudAggregatorNode.cpp
+37
View File
@@ -0,0 +1,37 @@
/*
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_util/point_cloud_aggregator.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/PointCloudAssemblerNode.cpp
+40
View File
@@ -0,0 +1,40 @@
/*
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_util/point_cloud_assembler.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/PointCloudToDepthImageNode.cpp
+41
View File
@@ -0,0 +1,41 @@
/*
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/utilite/ULogger.h>
#include "rtabmap_util/pointcloud_to_depthimage.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/PointCloudXYZNode.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_util/point_cloud_xyz.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudXYZ>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/PointCloudXYZRGBNode.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_util/point_cloud_xyzrgb.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudXYZRGB>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/RGBDRelayNode.cpp
+38
View File
@@ -0,0 +1,38 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include "rtabmap_util/rgbd_relay.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()));
rclcpp::shutdown();
return 0;
}
rtabmap_ros/rtabmap_util/src/RGBDSplitNode.cpp
+37
View File
@@ -0,0 +1,37 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include "rtabmap_util/rgbd_split.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
rclcpp::shutdown();
}
rtabmap_ros/rtabmap_util/src/nodelets/disparity_to_depth.cpp
+145
View File
@@ -0,0 +1,145 @@
/*
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_util/disparity_to_depth.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <image_transport/image_transport.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_util
{
DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
rclcpp::Node("disparity_to_depth", options)
{
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
}
DisparityToDepth::~DisparityToDepth(){}
void DisparityToDepth::callback(const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1");
return;
}
bool publish32f = pub32f_.getNumSubscribers();
bool publish16u = pub16u_.getNumSubscribers();
if(publish32f || publish16u)
{
// sensor_msgs::image_encodings::TYPE_32FC1
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
cv::Mat depth32f;
cv::Mat depth16u;
if(publish32f)
{
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
}
if(publish16u)
{
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
}
float * depth32fPtr=0;
unsigned short * depth16uPtr=0;
for (int i = 0; i < disparity.rows; ++i)
{
const float * rowPtr = (const float*)disparity.ptr(i);
if(publish32f)
{
depth32fPtr = (float*)depth32f.ptr(i);
}
if(publish16u)
{
depth16uPtr = (unsigned short*)depth16u.ptr(i);
}
for (int j = 0; j < disparity.cols; ++j)
{
const float & disparity_value = rowPtr[j];
if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
{
// baseline * focal / disparity
float depth = disparityMsg->t * disparityMsg->f / disparity_value;
if(publish32f)
{
depth32fPtr[j] = depth;
}
if(publish16u)
{
depth16uPtr[j] = (unsigned short)(depth*1000.0f);
}
}
}
}
if(publish32f)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
sensor_msgs::msg::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub32f_.publish(depthMsg);
}
if(publish16u)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
sensor_msgs::msg::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub16u_.publish(depthMsg);
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::DisparityToDepth)
rtabmap_ros/rtabmap_util/src/nodelets/imu_to_tf.cpp
+115
View File
@@ -0,0 +1,115 @@
/*
Copyright (c) 2010-2019, 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_util/imu_to_tf.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/utils.hpp>
namespace rtabmap_util
{
ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) :
rclcpp::Node("imu_to_tf", options),
fixedFrameId_("odom"),
waitForTransformDuration_(0.1)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_);
qos = this->declare_parameter("qos", qos);
waitForTransformDuration_ = this->declare_parameter("wait_for_transform_duration", waitForTransformDuration_);
RCLCPP_INFO(this->get_logger(), "fixed_frame_id: %s", fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "base_frame_id: %s", baseFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "qos: %d", qos);
sub_ = create_subscription<sensor_msgs::msg::Imu>("imu/data", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ImuToTF::imuCallback, this, std::placeholders::_1));
}
ImuToTF::~ImuToTF()
{
}
void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
{
tf2::Quaternion q;
tf2::fromMsg(msg->orientation, q);
tf2::Transform st(q);
std::string childFrameId = msg->header.frame_id;
if(!baseFrameId_.empty() &&
baseFrameId_.compare(msg->header.frame_id) != 0)
{
try
{
std::string errorMsg;
if(!tfBuffer_->canTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(waitForTransformDuration_), &errorMsg))
{
RCLCPP_ERROR(this->get_logger(), "Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, rtabmap_conversions::timestampFromROS(msg->header.stamp), errorMsg.c_str());
return;
}
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp);
tf2::Transform tmp_t;
tf2::fromMsg(tmp.transform, tmp_t);
tf2::Quaternion q;
q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation()));
tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation
st.setRotation(t.getRotation());
childFrameId = baseFrameId_;
}
catch(tf2::TransformException & ex)
{
RCLCPP_ERROR(this->get_logger(), "(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
return;
}
}
geometry_msgs::msg::TransformStamped output;
output.header.frame_id = fixedFrameId_;
output.header.stamp = msg->header.stamp;
output.child_frame_id = childFrameId;
output.transform = tf2::toMsg(st);
tfBroadcaster_->sendTransform(output);
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::ImuToTF)
rtabmap_ros/rtabmap_util/src/nodelets/lidar_deskewing.cpp
+103
View File
@@ -0,0 +1,103 @@
#include <rtabmap_util/lidar_deskewing.hpp>
#include <laser_geometry/laser_geometry.hpp>
#include <rtabmap_conversions/MsgConversion.h>
namespace rtabmap_util
{
LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
Node("lidar_deskewing", options),
waitForTransformDuration_(0.01),
slerp_(false)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 5;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
slerp_ = this->declare_parameter("slerp", slerp_);
RCLCPP_INFO(this->get_logger(), " fixed_frame_id: %s", fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), " wait_for_transform: %fs", waitForTransformDuration_);
RCLCPP_INFO(this->get_logger(), " slerp: %s", slerp_?"true":"false");
if(fixedFrameId_.empty())
{
RCLCPP_FATAL(this->get_logger(), "fixed_frame_id parameter cannot be empty!");
}
subScan_ = create_subscription<sensor_msgs::msg::LaserScan>("input_scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackScan, this, std::placeholders::_1));
subCloud_ = create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackCloud, this, std::placeholders::_1));
pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
}
LidarDeskewing::~LidarDeskewing()
{
}
void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
{
// make sure the frame of the laser is updated during the whole scan time
rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
msg->header.frame_id,
fixedFrameId_,
msg->header.stamp,
rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
*tfBuffer_,
waitForTransformDuration_);
if(tmpT.isNull())
{
return;
}
sensor_msgs::msg::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfBuffer_);
rtabmap::Transform t = rtabmap_conversions::getTransform(msg->header.frame_id, scanOut.header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransformDuration_);
if(t.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
scanOut.header.frame_id.c_str(), msg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(msg->header.stamp));
return;
}
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
scanOutDeskewed.header.frame_id = msg->header.frame_id;
pubScan_->publish(scanOutDeskewed);
}
void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
{
sensor_msgs::msg::PointCloud2 msgDeskewed;
if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_))
{
pubCloud_->publish(msgDeskewed);
}
else
{
// Just republish the msg to not breakdown downstream
// A warning should be already shown (see deskew() source code)
RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!");
pubCloud_->publish(*msg);
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::LidarDeskewing)
rtabmap_ros/rtabmap_util/src/nodelets/map_assembler.cpp
+361
View File
@@ -0,0 +1,361 @@
/*
Copyright (c) 2010-2024, 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_util/map_assembler.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
#ifdef PRE_ROS_JAZZY
namespace rclcpp{
rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;}
}
#endif
using namespace std::chrono_literals;
namespace rtabmap_util
{
MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
Node("map_assembler", options),
rtabmapNodeName_("rtabmap"),
localGridsRegenerated_(false)
{
std::string configPath;
configPath = this->declare_parameter("config_path", configPath);
localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
//parameters
rtabmap::ParametersMap parameters;
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
{
RCLCPP_INFO(this->get_logger(), "MapAssembler: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters;
rtabmap::Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = allParameters.find(iter->first);
if(jter!=allParameters.end())
{
iter->second = jter->second;
}
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Config file \"%s\" not found!", configPath.c_str());
}
}
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rclcpp::Parameter parameter;
std::string vStr = this->declare_parameter(iter->first, iter->second);
if(vStr.compare(iter->second)!=0)
{
RCLCPP_INFO(this->get_logger(), "MapAssembler: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
}
std::vector<std::string> tmpList = this->get_node_options().arguments();
std::vector<std::string> argList;
for(unsigned int i=0; i<tmpList.size(); ++i)
{
// Issue with ros2 launch files in which we cannot pass a
// list of strings as argument (they will appear in same string)
std::list<std::string> v = uSplit(tmpList[i]);
for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
{
argList.push_back(*iter);
}
}
char ** argv = new char*[argList.size()];
for(unsigned int i=0; i<argList.size(); ++i)
{
argv[i] = &argList[i].at(0);
}
rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
delete [] argv;
for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
if(jter!=parameters.end())
{
RCLCPP_INFO(this->get_logger(), "MapAssembler: Update parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
jter->second = iter->second;
}
else
{
RCLCPP_INFO(this->get_logger(), "MapAssembler: Ignored parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
}
}
// Backward compatibility
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
iter!=rtabmap::Parameters::getRemovedParameters().end();
++iter)
{
rclcpp::Parameter parameter;
if(get_parameter(iter->first, parameter))
{
std::string vStr = parameter.as_string();
if(!iter->second.second.empty() && parameters.find(iter->second.second)!=parameters.end())
{
RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
iter->first.c_str(), iter->second.second.c_str(), parameters.find(iter->second.second)->second.c_str(), vStr.c_str());
}
else if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
{
// can be migrated
parameters.at(iter->second.second)= vStr;
RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
}
else
{
if(iter->second.second.empty())
{
RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
}
}
RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids = %s", this->get_name(), localGridsRegenerated_?"true":"false");
mapsManager_.init(*this, this->get_name(), true);
mapsManager_.backwardCompatibilityParameters(*this, parameters);
mapsManager_.setParameters(parameters);
const std::string servicePrefix = get_name() + std::string("/");
resetService_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&MapAssembler::reset, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&MapAssembler::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&MapAssembler::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
#endif
#endif
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
}
MapAssembler::~MapAssembler() {}
void MapAssembler::timerCallback()
{
// Just do this callback one time
timer_->cancel();
if(mapDataSub_.get())
{
// double call? ignore
return;
}
std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
if(client_->wait_for_service(5s))
{
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
request->global_map = false;
request->optimized = true;
request->graph_only = false;
auto future = client_->async_send_request(request);
std::future_status status = future.wait_for(10s);
if (status == std::future_status::ready) {
RCLCPP_INFO(this->get_logger(), "Initializing cache...");
processMapData(future.get()->data);
RCLCPP_INFO(this->get_logger(), "Initializing cache... done! The map"
" will be assembled on next subscriber connection.");
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not responding after waiting for 10 seconds.",
getMapSrv.c_str());
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
"may not be a problem if rtabmap is started afterwards. If rtabmap "
"is started after in localization mode, call %s/publish_maps "
"service with graph_only=false to make sure map_assembler has all the data.",
getMapSrv.c_str(),
rtabmapNodeName_.c_str());
}
rclcpp::SubscriptionOptions options;
options.callback_group = timerCbGroup_;
mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
}
void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)
{
processMapData(*msg);
}
void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
{
UTimer timer;
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> constraints;
rtabmap::Transform mapOdom;
rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
for(unsigned int i=0; i<msg.nodes.size(); ++i)
{
if(msg.nodes[i].data.left_compressed.size() ||
msg.nodes[i].data.right_compressed.size() ||
msg.nodes[i].data.laser_scan_compressed.size())
{
rtabmap::Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
if(localGridsRegenerated_)
{
data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
}
uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
}
}
// create a tmp signature with latest sensory data
if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
{
rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first);
rtabmap::SensorData tmpData = tmpS.sensorData();
tmpData.setId(0);
uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData)));
poses.insert(std::make_pair(0, poses.rbegin()->second));
}
// Update maps
if(!nodes_.empty())
{
poses = mapsManager_.updateMapCaches(
poses,
0,
false,
false,
nodes_);
}
double updateTime = timer.ticks();
mapFrameId_ = msg.header.frame_id;
optimizedPoses_ = poses;
mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
RCLCPP_INFO(this->get_logger(), "map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
}
void MapAssembler::reset(const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<std_srvs::srv::Empty::Request>,
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
RCLCPP_INFO(this->get_logger(), "map_assembler: reset!");
mapsManager_.clear();
}
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
void MapAssembler::octomapBinaryCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
{
RCLCPP_INFO(this->get_logger(), "Sending binary map data on service request");
res->map.header.frame_id = mapFrameId_;
res->map.header.stamp = now();
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
if(octomap->octree()->size())
octomap_msgs::binaryMapToMsg(*octomap->octree(), res->map);
}
void MapAssembler::octomapFullCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
{
RCLCPP_INFO(this->get_logger(), "Sending full map data on service request");
res->map.header.frame_id = mapFrameId_;
res->map.header.stamp = now();
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
if(octomap->octree()->size())
octomap_msgs::fullMapToMsg(*octomap->octree(), res->map);
}
#endif
#endif
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::MapAssembler)
rtabmap_ros/rtabmap_util/src/nodelets/obstacles_detection.cpp
+314
View File
@@ -0,0 +1,314 @@
/*
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_util/obstacles_detection.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <rtabmap/core/LocalGridMaker.h>
#include <rtabmap_conversions/MsgConversion.h>
#include "rtabmap/utilite/UStl.h"
namespace rtabmap_util
{
ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
Node("obstacles_detection", options),
frameId_("base_link"),
waitForTransform_(0.2),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
warned_(false),
rangeMin_(0),
rangeMax_(0)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
frameId_ = this->declare_parameter("frame_id", frameId_);
mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
{
std::string vStr = declare_parameter(iter->first, iter->second);
if(vStr.compare(iter->second) != 0)
{
RCLCPP_INFO(this->get_logger(), "obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
}
UASSERT(uContains(gridParameters, rtabmap::Parameters::kGridMapFrameProjection()));
mapFrameProjection_ = uStr2Bool(gridParameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
if(mapFrameProjection_ && mapFrameId_.empty())
{
RCLCPP_ERROR(this->get_logger(), "obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
}
localMapMaker_.parseParameters(gridParameters);
rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMin(), rangeMin_);
rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMax(), rangeMax_);
tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
}
pcl::PointCloud<pcl::PointXYZ> rangeFiltering(
const pcl::PointCloud<pcl::PointXYZ> & cloud,
float rangeMin,
float rangeMax)
{
if(!cloud.empty() && (rangeMin > 0.0f || rangeMax > 0.0f))
{
pcl::PointCloud<pcl::PointXYZ> output;
output.reserve(cloud.size());
int oi = 0;
float rangeMinSqrd = rangeMin * rangeMin;
float rangeMaxSqrd = rangeMax * rangeMax;
for(size_t i=0; i<cloud.size(); ++i)
{
const pcl::PointXYZ & pt = cloud.at(i);
float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
if(rangeMin > 0.0f && r < rangeMinSqrd)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
}
output.push_back(pt);
++oi;
}
output.resize(oi);
return output;
}
return cloud;
}
void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
{
rclcpp::Time time = now();
if (groundPub_->get_subscription_count() == 0 && obstaclesPub_->get_subscription_count() == 0 && projObstaclesPub_->get_subscription_count() == 0)
{
// no one wants the results
return;
}
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
localTransform = rtabmap_conversions::getTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
if(localTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
rtabmap::Transform pose = rtabmap::Transform::getIdentity();
if(!mapFrameId_.empty())
{
pose = rtabmap_conversions::getTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
if(pose.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", mapFrameId_.c_str(), frameId_.c_str());
return;
}
}
UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *inputCloud);
if(inputCloud->isOrganized())
{
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
}
else if(!inputCloud->is_dense && inputCloud->height == 1)
{
if(!warned_)
{
RCLCPP_WARN(this->get_logger(), "Detected possible wrong format of point cloud \"%s\", it is "
"indicated that it is not dense, but there is only one row. "
"Assuming it is dense... This message will only appear once.", cloudSub_->get_topic_name());
warned_ = true;
}
inputCloud->is_dense = true;
}
if(rangeMin_ > 0.0f || rangeMax_ > 0.0f)
{
*inputCloud = rangeFiltering(*inputCloud, rangeMin_, rangeMax_);
}
//Common variables for all strategies
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
if(inputCloud->size())
{
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
pcl::IndicesPtr flatObstacles(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = localMapMaker_.segmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
pose,
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
ground,
obstacles,
&flatObstacles);
if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
{
if(groundPub_->get_subscription_count() &&
ground.get() && ground->size())
{
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
}
if((obstaclesPub_->get_subscription_count() || projObstaclesPub_->get_subscription_count()) &&
obstacles.get() && obstacles->size())
{
// remove flat obstacles from obstacles
std::set<int> flatObstaclesSet;
if(projObstaclesPub_->get_subscription_count())
{
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
}
obstaclesCloud->resize(obstacles->size());
obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
int oi=0;
for(unsigned int i=0; i<obstacles->size(); ++i)
{
obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
if(flatObstaclesSet.size() == 0 ||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
{
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
++oi;
}
}
obstaclesCloudWithoutFlatSurfaces->resize(oi);
}
if(!localTransform.isIdentity() || !pose.isIdentity())
{
//transform back in topic frame for 3d clouds and base frame for 2d clouds
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
{
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
}
t = (t*localTransform).inverse();
if(groundCloud->size())
{
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
}
if(obstaclesCloud->size())
{
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
}
}
}
}
else
{
RCLCPP_WARN(this->get_logger(), "obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
}
if(groundPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*groundCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
groundPub_->publish(std::move(rosCloud));
}
if(obstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
obstaclesPub_->publish(std::move(rosCloud));
}
if(projObstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
rosCloud->header.stamp = cloudMsg->header.stamp;
rosCloud->header.frame_id = frameId_;
//publish the message
projObstaclesPub_->publish(std::move(rosCloud));
}
RCLCPP_DEBUG(this->get_logger(), "Obstacles segmentation time = %f s", (now() - time).seconds());
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::ObstaclesDetection)
rtabmap_ros/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+431
View File
@@ -0,0 +1,431 @@
/*
Copyright (c) 2010-2019, 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_util/point_cloud_aggregator.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_filtering.h>
namespace rtabmap_util
{
PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
Node("point_cloud_aggregator", options),
warningThread_(0),
callbackCalled_(false),
exactSync4_(0),
approxSync4_(0),
exactSync3_(0),
approxSync3_(0),
exactSync2_(0),
approxSync2_(0),
waitForTransform_(0.1),
xyzOutput_(false)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int topicQueueSize = 1;
int syncQueueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
int qos=RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
frameId_ = this->declare_parameter("frame_id", frameId_);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
approx = this->declare_parameter("approx_sync", approx);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
count = this->declare_parameter("count", count);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
xyzOutput_ = this->declare_parameter("xyz_output", xyzOutput_);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
std::string subscribedTopicsMsg;
if(count == 4)
{
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name(),
cloudSub_3_.getSubscriber()->get_topic_name(),
cloudSub_4_.getSubscriber()->get_topic_name());
}
else if(count == 3)
{
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name(),
cloudSub_3_.getSubscriber()->get_topic_name());
}
else
{
if(approx)
{
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
exactSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getSubscriber()->get_topic_name(),
cloudSub_2_.getSubscriber()->get_topic_name());
}
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1.0/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
this->get_name(),
approx?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
}
}
});
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
}
PointCloudAggregator::~PointCloudAggregator()
{
delete exactSync4_;
delete approxSync4_;
delete exactSync3_;
delete approxSync3_;
delete exactSync2_;
delete approxSync2_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
}
void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_4)
{
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
clouds.push_back(cloudMsg_1);
clouds.push_back(cloudMsg_2);
clouds.push_back(cloudMsg_3);
clouds.push_back(cloudMsg_4);
combineClouds(clouds);
}
void PointCloudAggregator::clouds3_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3)
{
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
clouds.push_back(cloudMsg_1);
clouds.push_back(cloudMsg_2);
clouds.push_back(cloudMsg_3);
combineClouds(clouds);
}
void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2)
{
std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
clouds.push_back(cloudMsg_1);
clouds.push_back(cloudMsg_2);
combineClouds(clouds);
}
void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
{
callbackCalled_ = true;
UASSERT(cloudMsgs.size() > 1);
if(cloudPub_->get_subscription_count())
{
pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
std::string frameId = frameId_;
if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
{
sensor_msgs::msg::PointCloud2 tmp;
rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, waitForTransform_);
if(t.isNull())
{
return;
}
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
pcl_conversions::toPCL(tmp, *output);
}
else
{
pcl_conversions::toPCL(*cloudMsgs[0], *output);
frameId = cloudMsgs[0]->header.frame_id;
}
if(xyzOutput_ && !output->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<output->fields.size(); ++i)
{
if(output->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(output->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(output->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*output, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *output);
}
}
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
{
rtabmap::Transform cloudDisplacement;
if(!fixedFrameId_.empty() &&
cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
{
// approx sync
cloudDisplacement = rtabmap_conversions::getMovingTransform(
frameId, //sourceTargetFrame
fixedFrameId_, //fixedFrame
cloudMsgs[0]->header.stamp, //stampTarget
cloudMsgs[i]->header.stamp, //stampSource
*tfBuffer_,
waitForTransform_);
}
pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
{
sensor_msgs::msg::PointCloud2 tmp;
rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, waitForTransform_);
rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
if(!cloudDisplacement.isNull())
{
sensor_msgs::msg::PointCloud2 tmp2;
rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
pcl_conversions::toPCL(tmp2, *cloud2);
}
else
{
pcl_conversions::toPCL(tmp, *cloud2);
}
}
else
{
if(!cloudDisplacement.isNull())
{
sensor_msgs::msg::PointCloud2 tmp;
rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
pcl_conversions::toPCL(tmp, *cloud2);
}
else
{
pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
}
}
if(!cloud2->is_dense)
{
// remove nans
cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
}
if(xyzOutput_ && !cloud2->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<cloud2->fields.size(); ++i)
{
if(cloud2->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(cloud2->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(cloud2->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *cloud2);
}
}
if(output->data.empty())
{
output = cloud2;
}
else if(!cloud2->data.empty())
{
if(output->fields.size() != cloud2->fields.size())
{
RCLCPP_WARN(this->get_logger(), "%s: Input topics don't have all the "
"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
"may fails. You can enable \"xyz_output\" option "
"to convert all inputs to XYZ.",
get_name(),
(int)output->fields.size(),
i+1,
(int)output->fields.size());
}
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
pcl::concatenate(*output, *cloud2, *tmp_output);
#else
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
#endif
//Make sure row_step is the sum of both
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
}
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl_conversions::moveFromPCL(*output, *rosCloud);
rosCloud->header.stamp = cloudMsgs[0]->header.stamp;
rosCloud->header.frame_id = frameId;
cloudPub_->publish(std::move(rosCloud));
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::PointCloudAggregator)
rtabmap_ros/rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+534
View File
@@ -0,0 +1,534 @@
/*
Copyright (c) 2010-2019, 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_util/point_cloud_assembler.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/Version.h>
namespace rtabmap_util
{
PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
Node("point_cloud_assembler", options),
warningThread_(0),
callbackCalled_(false),
exactSync_(0),
exactInfoSync_(0),
maxClouds_(0),
skipClouds_(0),
cloudsSkipped_(0),
circularBuffer_(false),
linearUpdate_(0),
angularUpdate_(0),
assemblingTime_(0),
waitForTransform_(0.1),
rangeMin_(0),
rangeMax_(0),
voxelSize_(0),
noiseRadius_(0),
noiseMinNeighbors_(5),
removeZ_(false),
fixedFrameId_("odom"),
frameId_("")
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int topicQueueSize = 10;
int syncQueueSize = 10;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool subscribeOdomInfo = false;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosOdom = this->declare_parameter("qos_odom", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
frameId_ = this->declare_parameter("frame_id", frameId_);
maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
assemblingTime_ = this->declare_parameter("assembling_time", assemblingTime_);
skipClouds_ = this->declare_parameter("skip_clouds", skipClouds_);
circularBuffer_ = this->declare_parameter("circular_buffer", circularBuffer_);
linearUpdate_ = this->declare_parameter("linear_update", linearUpdate_);
angularUpdate_ = this->declare_parameter("angular_update", angularUpdate_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
rangeMin_ = this->declare_parameter("range_min", rangeMin_);
rangeMax_ = this->declare_parameter("range_max", rangeMax_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
noiseRadius_ = this->declare_parameter("noise_radius", noiseRadius_);
noiseMinNeighbors_ = this->declare_parameter("noise_min_neighbors", noiseMinNeighbors_);
removeZ_ = this->declare_parameter("remove_z", removeZ_);
subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size=%d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size=%d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos=%d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_odom=%d", get_name(), qosOdom);
RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "%s: frame_id=%s", get_name(), frameId_.c_str());
RCLCPP_INFO(this->get_logger(), "%s: max_clouds=%d", get_name(), maxClouds_);
RCLCPP_INFO(this->get_logger(), "%s: assembling_time=%fs", get_name(), assemblingTime_);
RCLCPP_INFO(this->get_logger(), "%s: skip_clouds=%d", get_name(), skipClouds_);
RCLCPP_INFO(this->get_logger(), "%s: circular_buffer=%s", get_name(), circularBuffer_?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: linear_update=%f m", get_name(), linearUpdate_);
RCLCPP_INFO(this->get_logger(), "%s: angular_update=%f rad", get_name(), angularUpdate_);
RCLCPP_INFO(this->get_logger(), "%s: wait_for_transform=%f", get_name(), waitForTransform_);
RCLCPP_INFO(this->get_logger(), "%s: range_min=%f", get_name(), rangeMin_);
RCLCPP_INFO(this->get_logger(), "%s: range_max=%f", get_name(), rangeMax_);
RCLCPP_INFO(this->get_logger(), "%s: voxel_size=%fm", get_name(), voxelSize_);
RCLCPP_INFO(this->get_logger(), "%s: noise_radius=%fm", get_name(), noiseRadius_);
RCLCPP_INFO(this->get_logger(), "%s: noise_min_neighbors=%d", get_name(), noiseMinNeighbors_);
RCLCPP_INFO(this->get_logger(), "%s: remove_z=%s", get_name(), removeZ_?"true":"false");
if(maxClouds_==0 && assemblingTime_ ==0.0)
{
RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_clouds or assembling_time parameters should be set!");
exit(-1);
}
cloudsSkipped_ = skipClouds_;
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
if(!fixedFrameId_.empty())
{
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s",
get_name(),
cloudSub_->get_topic_name());
}
else if(subscribeOdomInfo)
{
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
syncCloudSub_.getSubscriber()->get_topic_name(),
syncOdomSub_.getSubscriber()->get_topic_name(),
syncOdomInfoSub_.getSubscriber()->get_topic_name());
}
else
{
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_);
exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
syncCloudSub_.getSubscriber()->get_topic_name(),
syncOdomSub_.getSubscriber()->get_topic_name());
}
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1.0/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
RCLCPP_WARN(this->get_logger(),
"%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. %s",
get_name(),
subscribedTopicsMsg_.c_str());
}
}
});
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
}
PointCloudAssembler::~PointCloudAssembler()
{
delete exactSync_;
delete exactInfoSync_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
}
void PointCloudAssembler::callbackCloudOdom(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
{
callbackCalled_ = true;
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
if(!odom.isNull())
{
fixedFrameId_ = odomMsg->header.frame_id;
callbackCloud(cloudMsg);
}
else
{
RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
clouds_.clear();
}
}
sensor_msgs::msg::PointCloud2 removeField(const sensor_msgs::msg::PointCloud2 & input, const std::string & field)
{
sensor_msgs::msg::PointCloud2 output;
int offset = 0;
std::vector<int> inputFieldIndex;
for(size_t i=0; i<input.fields.size(); ++i)
{
if(input.fields[i].name.compare(field) == 0)
{
continue;
}
else
{
sensor_msgs::msg::PointField outputField = input.fields[i];
outputField.offset = offset;
offset += outputField.count * rtabmap_conversions::sizeOfPointField(outputField.datatype);
output.fields.push_back(outputField);
inputFieldIndex.push_back(i);
}
}
output.header = input.header;
output.height = input.height;
output.width = input.width;
output.is_bigendian = input.is_bigendian;
output.is_dense = input.is_dense;
output.point_step = offset;
output.row_step = output.width * output.point_step;
output.data.resize(output.height*output.row_step);
int total = output.height*output.width;
for(int i=0; i<total; ++i)
{
// for each point, copy fields
int oi = i*output.point_step;
int pi = i*input.point_step;
for(size_t j=0;j<output.fields.size(); ++j)
{
memcpy(&output.data[oi + output.fields[j].offset],
&input.data[pi + input.fields[inputFieldIndex[j]].offset],
output.fields[j].count * rtabmap_conversions::sizeOfPointField(output.fields[j].datatype));
}
}
return output;
}
void PointCloudAssembler::callbackCloudOdomInfo(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
callbackCalled_ = true;
rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
if(!odom.isNull())
{
if(odomInfoMsg->key_frame_added)
{
fixedFrameId_ = odomMsg->header.frame_id;
callbackCloud(cloudMsg);
}
else
{
RCLCPP_INFO(this->get_logger(), "Skipping non keyframe...");
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Resetting point cloud assembler as null odometry has been received.");
clouds_.clear();
}
}
void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
{
callbackCalled_ = true;
if(cloudPub_->get_subscription_count())
{
UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
{
cloudsSkipped_ = 0;
rtabmap::Transform pose = rtabmap_conversions::getTransform(
fixedFrameId_, //fromFrame
cloudMsg->header.frame_id, //toFrame
cloudMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
if(pose.isNull())
{
RCLCPP_ERROR(get_logger(), "Cloud not transform all clouds! Resetting...");
clouds_.clear();
return;
}
bool isMoving = true;
if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
{
rtabmap::Transform delta = previousPose_.inverse()*pose;
float roll, pitch, yaw;
delta.getEulerAngles(roll, pitch, yaw);
isMoving = fabs(delta.x()) > linearUpdate_ ||
fabs(delta.y()) > linearUpdate_ ||
fabs(delta.z()) > linearUpdate_ ||
(angularUpdate_>0.0f && (
fabs(roll) > angularUpdate_ ||
fabs(pitch) > angularUpdate_ ||
fabs(yaw) > angularUpdate_));
}
pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
{
pcl_conversions::toPCL(*cloudMsg, *newCloud);
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
std::uint64_t stamp = newCloud->header.stamp;
#else
pcl::uint64_t stamp = newCloud->header.stamp;
#endif
newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
newCloud->header.stamp = stamp;
}
else
{
sensor_msgs::msg::PointCloud2 output;
rtabmap_conversions::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
pcl_conversions::toPCL(output, *newCloud);
}
if(!newCloud->is_dense)
{
// remove nans
newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
}
clouds_.push_back(newCloud);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
bool reachedMaxSize =
((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
||
((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
#else
bool reachedMaxSize =
((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
||
((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
#endif
if( circularBuffer_ || reachedMaxSize )
{
pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
{
if(assembled->data.empty())
{
*assembled = *(*iter);
}
else
{
pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
pcl::concatenate(*assembled, *(*iter), *assembledTmp);
#else
pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
#endif
//Make sure row_step is the sum of both
assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
assembled = assembledTmp;
}
}
sensor_msgs::msg::PointCloud2 rosCloud;
if(voxelSize_>0.0)
{
// estimate if there would be an overflow
int x_idx=-1, y_idx=-1, z_idx=-1;
for (std::size_t d = 0; d < assembled->fields.size (); ++d)
{
if (assembled->fields[d].name.compare("x")==0)
x_idx = d;
if (assembled->fields[d].name.compare("y")==0)
y_idx = d;
if (assembled->fields[d].name.compare("z")==0)
z_idx = d;
}
bool overflow = false;
if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
Eigen::Vector4f min_p, max_p;
pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
float inverseVoxelSize = 1.0f/voxelSize_;
std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
{
overflow = true;
}
}
if(overflow)
{
rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
std::uint64_t stamp = assembled->header.stamp;
#else
pcl::uint64_t stamp = assembled->header.stamp;
#endif
assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
assembled->header.stamp = stamp;
}
else
{
pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
filter.setInputCloud(assembled);
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
filter.filter(*output);
assembled = output;
}
}
if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
{
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
filter.setRadiusSearch(noiseRadius_);
filter.setMinNeighborsInRadius(noiseMinNeighbors_);
filter.setInputCloud(assembled);
pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
filter.filter(*output);
assembled = output;
}
pcl_conversions::moveFromPCL(*assembled, rosCloud);
rtabmap::Transform t = pose;
if(!frameId_.empty())
{
// transform in target frame_id instead of sensor frame
t = rtabmap_conversions::getTransform(
fixedFrameId_, //fromFrame
frameId_, //toFrame
cloudMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
if(t.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
clouds_.clear();
return;
}
}
rtabmap_conversions::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
if(removeZ_)
{
rosCloud = removeField(rosCloud, "z");
}
rosCloud.header = cloudMsg->header;
if(!frameId_.empty())
{
rosCloud.header.frame_id = frameId_;
}
cloudPub_->publish(rosCloud);
if(circularBuffer_)
{
if(!isMoving)
{
clouds_.pop_back();
}
else
{
previousPose_ = pose;
if(reachedMaxSize)
{
clouds_.pop_front();
}
}
}
else
{
clouds_.clear();
previousPose_.setNull();
}
}
else if(!isMoving)
{
clouds_.pop_back();
}
else
{
previousPose_ = pose;
}
}
else
{
++cloudsSkipped_;
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::PointCloudAssembler)
rtabmap_ros/rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+360
View File
@@ -0,0 +1,360 @@
/*
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_util/point_cloud_xyz.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <pcl_conversions/pcl_conversions.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#include <image_geometry/pinhole_camera_model.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap_util
{
PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
Node("point_cloud_xyz", options),
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0)
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
decimation_ = this->declare_parameter("decimation", decimation_);
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
normalK_ = this->declare_parameter("normal_k", normalK_);
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr);
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
}
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
image_transport::TransportHints hints(this);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZ::~PointCloudXYZ()
{
delete approxSyncDepth_;
delete approxSyncDisparity_;
delete exactSyncDepth_;
delete exactSyncDisparity_;
}
void PointCloudXYZ::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb;
if(model.imageWidth() && model.imageHeight())
{
roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
}
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
(roiRgb.width != 0 ||
(roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)))
{
depth = cv::Mat(depth, roiDepth);
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
model = model.roi(roiRgb);
}
else
{
model = model.roi(roiDepth);
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth(
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depthMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZ::callbackDisparity(
const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
return;
}
cv::Mat disparity;
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
else
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparity(
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, disparityMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
pclCloud->is_dense = true;
}
// Do radius filtering after voxel filtering ( a lot faster)
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;
//publish the message
cloudPub_->publish(std::move(rosCloud));
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::PointCloudXYZ)
rtabmap_ros/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+525
View File
@@ -0,0 +1,525 @@
/*
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_util/point_cloud_xyzrgb.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_conversions/MsgConversion.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#include <image_geometry/pinhole_camera_model.hpp>
#include <image_geometry/stereo_camera_model.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap_util
{
PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
Node("point_cloud_xyzrgb", options),
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
approxSyncStereo_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0),
exactSyncStereo_(0)
{
bool approxSync = true;
std::string roiStr;
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
decimation_ = this->declare_parameter("decimation", decimation_);
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
normalK_ = this->declare_parameter("normal_k", normalK_);
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr);
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
}
// StereoBM parameters
stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
{
std::string vStr = declare_parameter(iter->first, iter->second);
if(vStr.compare(iter->second)!=0)
{
RCLCPP_INFO(this->get_logger(), "point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
}
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZRGB::~PointCloudXYZRGB()
{
delete approxSyncDepth_;
delete approxSyncDisparity_;
delete approxSyncStereo_;
delete exactSyncDepth_;
delete exactSyncDisparity_;
delete exactSyncStereo_;
}
void PointCloudXYZRGB::depthCallback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::Image::ConstSharedPtr imageDepth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
imagePtr = cv_bridge::toCvShare(image);
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Mat rgb = imagePtr->image;
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)
{
depth = cv::Mat(depth, roiDepth);
rgb = cv::Mat(rgb, roiRgb);
model = model.roi(roiRgb);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
rgb,
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imagePtr->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::disparityCallback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const stereo_msgs::msg::DisparityImage::ConstSharedPtr imageDisparity,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
imagePtr = cv_bridge::toCvShare(image);
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
return;
}
cv::Mat disparity;
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
}
else
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imageDisparity->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from disparity time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::stereoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoRight)
{
if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0) ||
!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (enc=%s)", imageLeft->encoding.c_str());
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
}
else
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
{
RCLCPP_WARN(this->get_logger(), "\"roi_ratios\" set but ignored for stereo images.");
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
rtabmap_conversions::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
processAndPublish(pclCloud, indices, imageLeft->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from stereo time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::rgbdImageCallback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
{
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(image);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(data.isValid())
{
pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
data,
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_,
roiRatios_);
processAndPublish(pclCloud, indices, image->header);
}
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from rgbd_image time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::processAndPublish(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud,
pcl::IndicesPtr & indices,
const std_msgs::msg::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
pclCloud->is_dense = true;
}
// Do radius filtering after voxel filtering ( a lot faster)
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;
//publish the message
cloudPub_->publish(std::move(rosCloud));
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::PointCloudXYZRGB)
rtabmap_ros/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+286
View File
@@ -0,0 +1,286 @@
/*
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_util/pointcloud_to_depthimage.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <sensor_msgs/image_encodings.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
namespace rtabmap_util
{
PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & options) :
Node("pointcloud_to_depthimage", options),
waitForTransform_(0.1),
fillHolesSize_ (0),
fillHolesError_(0.1),
fillIterations_(1),
decimation_(1),
upscale_(false),
upscaleDepthErrorRatio_(0.02),
approxSync_(0),
exactSync_(0)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int topicQueueSize = 10;
int syncQueueSize = 10;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool approx = true;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
fillHolesSize_ = this->declare_parameter("fill_holes_size", fillHolesSize_);
fillHolesError_ = this->declare_parameter("fill_holes_error", fillHolesError_);
fillIterations_ = this->declare_parameter("fill_iterations", fillIterations_);
decimation_ = this->declare_parameter("decimation", decimation_);
approx = this->declare_parameter("approx", approx);
upscale_ = this->declare_parameter("upscale", upscale_);
upscaleDepthErrorRatio_ = this->declare_parameter("upscale_depth_error_ratio", upscaleDepthErrorRatio_);
if(fixedFrameId_.empty() && approx)
{
RCLCPP_FATAL(this->get_logger(), "fixed_frame_id should be set when using approximate "
"time synchronization (approx=true)! If the robot "
"is moving, it could be \"odom\". If not moving, it "
"could be \"base_link\".");
}
RCLCPP_INFO(this->get_logger(), "Params:");
RCLCPP_INFO(this->get_logger(), " approx=%s", approx?"true":"false");
RCLCPP_INFO(this->get_logger(), " topic_queue_size=%d", topicQueueSize);
RCLCPP_INFO(this->get_logger(), " sync_queue_size=%d", syncQueueSize);
RCLCPP_INFO(this->get_logger(), " fixed_frame_id=%s", fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), " wait_for_transform=%fs", waitForTransform_);
RCLCPP_INFO(this->get_logger(), " fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
RCLCPP_INFO(this->get_logger(), " fill_holes_error=%f", fillHolesError_);
RCLCPP_INFO(this->get_logger(), " fill_iterations=%d", fillIterations_);
RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_);
RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
if(approx)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
fixedFrameId_.clear();
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudToDepthImage::~PointCloudToDepthImage()
{
delete approxSync_;
delete exactSync_;
}
void PointCloudToDepthImage::callback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointCloud2Msg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
{
if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
{
double cloudStamp = rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp);
double infoStamp = rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp);
rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
if(!fixedFrameId_.empty())
{
// approx sync
cloudDisplacement = rtabmap_conversions::getMovingTransform(
pointCloud2Msg->header.frame_id,
fixedFrameId_,
pointCloud2Msg->header.stamp,
cameraInfoMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
}
if(cloudDisplacement.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
pointCloud2Msg->header.frame_id.c_str(),
cameraInfoMsg->header.frame_id.c_str(),
fixedFrameId_.c_str());
return;
}
rtabmap::Transform cloudToCamera = rtabmap_conversions::getTransform(
pointCloud2Msg->header.frame_id,
cameraInfoMsg->header.frame_id,
cameraInfoMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
if(cloudToCamera.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!",
pointCloud2Msg->header.frame_id.c_str(),
cameraInfoMsg->header.frame_id.c_str());
return;
}
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfoMsg, localTransform);
sensor_msgs::msg::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
float scale = 1.0f/float(decimation_);
model = model.scaled(scale);
rtabmap_conversions::cameraModelToROS(model, cameraInfoMsgOut);
}
else
{
RCLCPP_ERROR(this->get_logger(), "decimation (%d) not valid for image size %dx%d",
decimation_,
model.imageWidth(),
model.imageHeight());
}
}
UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
cv_bridge::CvImage depthImage;
if(cloud->data.empty())
{
RCLCPP_WARN(this->get_logger(), "Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
}
else
{
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{
for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
if(pointCloudTransformedPub_->get_subscription_count()>0)
{
sensor_msgs::msg::PointCloud2 pointCloud2Out;
rtabmap_conversions::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
pointCloud2Out.header = cameraInfoMsg->header;
pointCloudTransformedPub_->publish(pointCloud2Out);
}
}
}
depthImage.header = cameraInfoMsg->header;
if(decimation_>1 && upscale_)
{
depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
}
if(depthImage32Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg());
if(cameraInfo32Pub_->get_subscription_count())
{
cameraInfo32Pub_->publish(cameraInfoMsgOut);
}
}
if(depthImage16Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg());
if(cameraInfo16Pub_->get_subscription_count())
{
cameraInfo16Pub_->publish(cameraInfoMsgOut);
}
}
if( cloudStamp != rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp) ||
infoStamp != rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp))
{
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"cloud=%f->%f info=%f->%f",
cloudStamp, rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp),
infoStamp, rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp));
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::PointCloudToDepthImage)
rtabmap_ros/rtabmap_util/src/nodelets/rgbd_relay.cpp
+166
View File
@@ -0,0 +1,166 @@
/*
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_util/rgbd_relay.hpp"
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap_conversions/MsgConversion.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/utilite/UConversion.h"
namespace rtabmap_util
{
RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
Node("rgbd_relay", options),
compress_(false),
uncompress_(false)
{
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
compress_ = this->declare_parameter("compress", compress_);
uncompress_ = this->declare_parameter("uncompress", uncompress_);
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
}
void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
{
if(rgbdImagePub_->get_subscription_count())
{
if(!compress_ && !uncompress_)
{
//just republish it
rgbdImagePub_->publish(*input);
return;
}
auto output = std::make_unique<rtabmap_msgs::msg::RGBDImage>();
output->header = input->header;
output->rgb_camera_info = input->rgb_camera_info;
output->depth_camera_info = input->depth_camera_info;
output->key_points = input->key_points;
output->points = input->points;
output->descriptors = input->descriptors;
output->global_descriptor = input->global_descriptor;
rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
if(compress_)
{
if(!input->rgb_compressed.data.empty())
{
// already compressed, just copy pointer
output->rgb_compressed = input->rgb_compressed;
}
else if(!input->rgb.data.empty())
{
cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG);
}
if(!input->depth_compressed.data.empty())
{
// already compressed, just copy pointer
output->depth_compressed = input->depth_compressed;
}
else if(!input->depth.data.empty())
{
if(stereoModel.isValidForProjection())
{
// right stereo image
cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG);
}
else
{
// depth image
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
output->depth_compressed.format = "png";
}
}
}
if(uncompress_)
{
if(!input->rgb.data.empty())
{
// already raw, just copy pointer
output->rgb = input->rgb;
}
if(!input->rgb_compressed.data.empty())
{
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
}
if(!input->depth.data.empty())
{
// already raw, just copy pointer
output->depth = input->depth;
}
else if(input->depth_compressed.format.compare("jpg")==0)
{
// right stereo image
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
}
else
{
// dpeth image
auto cvImg = std::make_unique<cv_bridge::CvImage>();
cvImg->header = input->depth_compressed.header;
cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
cvImg->toImageMsg(output->depth);
}
}
rgbdImagePub_->publish(std::move(output));
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::RGBDRelay)
rtabmap_ros/rtabmap_util/src/nodelets/rgbd_split.cpp
+111
View File
@@ -0,0 +1,111 @@
/*
Copyright (c) 2010-2022, 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_util/rgbd_split.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_util
{
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
Node("rgbd_split", options)
{
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
}
void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
{
if(rgbPub_.getNumSubscribers())
{
sensor_msgs::msg::Image outputImage;
sensor_msgs::msg::CameraInfo outputCameraInfo;
outputImage.header = outputCameraInfo.header = input->header;
outputCameraInfo = input->rgb_camera_info;
if(!input->rgb.data.empty())
{
// already raw, just copy pointer
outputImage = input->rgb;
}
else if(!input->rgb_compressed.data.empty())
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
#endif
}
rgbPub_.publish(outputImage, outputCameraInfo);
}
if(depthPub_.getNumSubscribers())
{
sensor_msgs::msg::Image outputImage;
sensor_msgs::msg::CameraInfo outputCameraInfo;
outputCameraInfo = input->depth_camera_info;
if(!input->depth.data.empty())
{
// already raw, just copy pointer
outputImage = input->depth;
}
else if(!input->depth_compressed.data.empty())
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
#endif
}
outputImage.header = outputCameraInfo.header = input->header;
depthPub_.publish(outputImage, outputCameraInfo);
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::RGBDSplit)