add validation check for scan-message (#1151)

* add validation check for scan-message Signed-off-by: goes <[email protected]> * remove abundant logger Signed-off-by: goes <[email protected]> * fit into main Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: goes <[email protected]> Signed-off-by: GoesM <[email protected]> Co-authored-by: goes <[email protected]> Co-authored-by: matlabbe <[email protected]>
introlab · Jul 1, 2024 · 680fe72 · 680fe72
1 parent 76c6a1f
commit 680fe72
Showing 1 changed file with 18 additions and 0 deletions.
diff --git 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,