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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+29 -12
View File
@@ -480,12 +480,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
return false;
}
void GuiWrapper::commonDepthCallback(
void GuiWrapper::commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr &,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -570,7 +571,8 @@ void GuiWrapper::commonDepthCallback(
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
LaserScan scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
@@ -585,18 +587,25 @@ void GuiWrapper::commonDepthCallback(
if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs.size() && depthMsgs[0].get())
{
ParametersMap allParameters = prefDialog_->getAllParameters();
bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
if(!rtabmap_ros::convertRGBDMsgs(
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
frameId,
odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp,
rgb,
depth,
cameraModels,
stereoCameraModels,
*tfBuffer_,
waitForTransform_))
waitForTransform_,
imagesAlreadyRectified))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
return;
@@ -653,13 +662,21 @@ void GuiWrapper::commonDepthCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
!stereoCameraModels.empty()?
rtabmap::SensorData(
scan,
rgb,
depth,
stereoCameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)):
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
info);
@@ -984,7 +1001,7 @@ void GuiWrapper::commonLaserScanCallback(
scan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
@@ -1056,7 +1073,7 @@ void GuiWrapper::commonOdomCallback(
rtabmap::SensorData(
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,