From 680fe729c5285b5a6aa6a1893f0073afe3d06267 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 1 Jul 2024 23:15:12 +0800 Subject: [PATCH 1/2] add validation check for scan-message (#1151) * add validation check for scan-message Signed-off-by: goes * remove abundant logger Signed-off-by: goes * fit into main Signed-off-by: GoesM --------- Signed-off-by: goes Signed-off-by: GoesM Co-authored-by: goes Co-authored-by: matlabbe --- rtabmap_conversions/src/MsgConversion.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 10250277..3903fc9c 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2591,6 +2591,24 @@ bool convertScanMsg( double waitForTransform, bool outputInFrameId) { + // scan message validation check + if(scan2dMsg.angle_increment == 0.0f) { + ROS_ERROR("convertScanMsg: angle_increment should not be 0!"); + return false; + } + if(scan2dMsg.range_min > scan2dMsg.range_max) { + ROS_ERROR("convertScanMsg: range_min (%f) should be smaller than range_max (%f)!", scan2dMsg.range_min, scan2dMsg.range_max); + return false; + } + if(scan2dMsg.angle_increment > 0 && scan2dMsg.angle_max < scan2dMsg.angle_min) { + ROS_ERROR("convertScanMsg: Angle increment (%f) should be negative if angle_min(%f) > angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + else if (scan2dMsg.angle_increment < 0 && scan2dMsg.angle_max > scan2dMsg.angle_min) { + ROS_ERROR("convertScanMsg: Angle increment (%f) should positive if angle_min(%f) < angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max); + return false; + } + // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = getMovingTransform( scan2dMsg.header.frame_id, From a97efff760720132ced2a607f384dc5d28a0d296 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jul 2024 16:20:34 -0700 Subject: [PATCH 2/2] Fixed #1186 --- .devcontainer/devcontainer.json | 3 +- rtabmap_odom/src/OdometryROS.cpp | 221 +++++++++++++++---------------- 2 files changed, 111 insertions(+), 113 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 3f90ca0d..13ff5c22 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,5 +7,6 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/catkin_ws", - "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'" + "postAttachCommand": "echo 'Initialize catkin: source /opt/ros/noetic/setup.bash && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'", + "runArgs": ["--privileged"] } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index b0277010..1219a87a 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -932,124 +932,121 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header } } - if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) + if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers()) { - if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers()) + rtabmap_msgs::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers()); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + if(odomSensorDataPub_.getNumSubscribers()) { - rtabmap_msgs::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers()); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - if(odomSensorDataPub_.getNumSubscribers()) - { - odomSensorDataPub_.publish(msg); - } - if(odomSensorDataFeaturesPub_.getNumSubscribers()) - { - // remove data - msg.left = sensor_msgs::Image(); - msg.right = sensor_msgs::Image(); - msg.laser_scan = sensor_msgs::PointCloud2(); - msg.grid_ground.clear(); - msg.grid_obstacles.clear(); - msg.grid_empty_cells.clear(); - odomSensorDataFeaturesPub_.publish(msg); - } + odomSensorDataPub_.publish(msg); } - if(odomSensorDataCompressedPub_.getNumSubscribers()) + if(odomSensorDataFeaturesPub_.getNumSubscribers()) { - cv::Mat compressedImage; - cv::Mat compressedDepth; - cv::Mat compressedScan; - if(compressionParallelized_) - { - rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); - rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); - if(!data.imageRaw().empty()) - { - ctImage.start(); - } - if(!data.depthOrRightRaw().empty()) - { - ctDepth.start(); - } - if(!data.laserScanRaw().isEmpty()) - { - ctLaserScan.start(); - } - ctImage.join(); - ctDepth.join(); - ctLaserScan.join(); - - compressedImage = ctImage.getCompressedData(); - compressedDepth = ctDepth.getCompressedData(); - compressedScan = ctLaserScan.getCompressedData(); - } - else - { - compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); - compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); - compressedScan = compressData2(data.laserScanRaw().data()); - } - if(!compressedImage.empty() && !data.stereoCameraModels().empty()) - { - data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); - } - else if(!compressedImage.empty() && !data.cameraModels().empty()) - { - data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); - } - if(!compressedScan.empty()) - { - data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? - LaserScan(compressedScan, - data.laserScanRaw().maxPoints(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().format(), - data.laserScanRaw().localTransform()): - LaserScan(compressedScan, - data.laserScanRaw().format(), - data.laserScanRaw().rangeMin(), - data.laserScanRaw().rangeMax(), - data.laserScanRaw().angleMin(), - data.laserScanRaw().angleMax(), - data.laserScanRaw().angleIncrement(), - data.laserScanRaw().localTransform()), false); - } - rtabmap_msgs::SensorData msg; - rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); - msg.header.stamp = header.stamp; // use corresponding time stamp to image - odomSensorDataCompressedPub_.publish(msg); + // remove data + msg.left = sensor_msgs::Image(); + msg.right = sensor_msgs::Image(); + msg.laser_scan = sensor_msgs::PointCloud2(); + msg.grid_ground.clear(); + msg.grid_obstacles.clear(); + msg.grid_empty_cells.clear(); + odomSensorDataFeaturesPub_.publish(msg); } - - if(visParams_) - { - if(icpParams_) - { - NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - else - { - NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - } - else // if(icpParams_) - { - NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); - } - - statusDiagnostic_.setStatus(pose.isNull()); - if(syncDiagnostic_.get() && !pose.isNull()) - { - double curentRate = 1.0/(ros::WallTime::now()-time).toSec(); - syncDiagnostic_->tick(header.stamp, - maxUpdateRate_>0 ? maxUpdateRate_: - expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); - } - - previousStamp_ = header.stamp.toSec(); } + if(odomSensorDataCompressedPub_.getNumSubscribers()) + { + cv::Mat compressedImage; + cv::Mat compressedDepth; + cv::Mat compressedScan; + if(compressionParallelized_) + { + rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); + rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); + if(!data.imageRaw().empty()) + { + ctImage.start(); + } + if(!data.depthOrRightRaw().empty()) + { + ctDepth.start(); + } + if(!data.laserScanRaw().isEmpty()) + { + ctLaserScan.start(); + } + ctImage.join(); + ctDepth.join(); + ctLaserScan.join(); + + compressedImage = ctImage.getCompressedData(); + compressedDepth = ctDepth.getCompressedData(); + compressedScan = ctLaserScan.getCompressedData(); + } + else + { + compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); + compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + compressedScan = compressData2(data.laserScanRaw().data()); + } + if(!compressedImage.empty() && !data.stereoCameraModels().empty()) + { + data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false); + } + else if(!compressedImage.empty() && !data.cameraModels().empty()) + { + data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false); + } + if(!compressedScan.empty()) + { + data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f? + LaserScan(compressedScan, + data.laserScanRaw().maxPoints(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().format(), + data.laserScanRaw().localTransform()): + LaserScan(compressedScan, + data.laserScanRaw().format(), + data.laserScanRaw().rangeMin(), + data.laserScanRaw().rangeMax(), + data.laserScanRaw().angleMin(), + data.laserScanRaw().angleMax(), + data.laserScanRaw().angleIncrement(), + data.laserScanRaw().localTransform()), false); + } + rtabmap_msgs::SensorData msg; + rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false); + msg.header.stamp = header.stamp; // use corresponding time stamp to image + odomSensorDataCompressedPub_.publish(msg); + } + + if(visParams_) + { + if(icpParams_) + { + NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + else + { + NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + } + else // if(icpParams_) + { + NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec()); + } + + statusDiagnostic_.setStatus(pose.isNull()); + if(syncDiagnostic_.get()) + { + double curentRate = 1.0/(ros::WallTime::now()-time).toSec(); + syncDiagnostic_->tick(header.stamp, + maxUpdateRate_>0 ? maxUpdateRate_: + expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: + previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); + } + + previousStamp_ = header.stamp.toSec(); } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)