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] 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,