added config for laserscan in lb-sim (#5174)

* added config for laserscan in lb-sim

Signed-off-by: RamanaBotta <ramanab@bosonmotors.com>

* fixing ament_flake8 errors

Signed-off-by: RamanaBotta <ramanab@bosonmotors.com>

* review: use_inf is default:true and added parameters on readme #4992

Signed-off-by: RamanaBotta <ramanab@bosonmotors.com>

* refactor: meaningfull value for scan_angle_increment

Signed-off-by: RamanaBotta <ramanab@bosonmotors.com>

---------

Signed-off-by: RamanaBotta <ramanab@bosonmotors.com>
Co-authored-by: RamanaBotta <ramanab@bosonmotors.com>

Author: RamanRobotics

nav2_bringup/params/nav2_params.yaml

@@ -565,3 +565,9 @@ loopback_simulator:
     map_frame_id: "map"
     scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
     update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

nav2_loopback_sim/README.md

@@ -42,6 +42,13 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py  # Nav2 integrated na
 - `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz)
 - `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`)
 - `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`)
+- `scan_range_min`: Minimum measurable distance from the scan in meters. Values below this are considered invalid (default: `0.05`)
+- `scan_range_max`: Maximum measurable distance from the scan in meters. Values beyond this are out of range (default: `30.0`)
+- `scan_angle_min`: Starting angle of the scan in radians (leftmost angle) (default: `-π` / `-3.1415`)
+- `scan_angle_max`: Ending angle of the scan in radians (rightmost angle) (default: `π` / `3.1415`)
+- `scan_angle_increment`: Angular resolution of the scan in radians (angle between consecutive measurements) (default: `0.02617`)
+- `scan_use_inf`: Whether to use `inf` for out-of-range values. If `false`, uses `scan_range_max - 0.1` instead (default: `True`)
+
 
 ### Topics
 

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -86,6 +86,30 @@ def __init__(self) -> None:
         self.declare_parameter('publish_clock', True)
         self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value
 
+        self.declare_parameter('scan_range_min',  0.05)
+        self.scan_range_min = \
+            self.get_parameter('scan_range_min').get_parameter_value().double_value
+
+        self.declare_parameter('scan_range_max',  30.0)
+        self.scan_range_max = \
+            self.get_parameter('scan_range_max').get_parameter_value().double_value
+
+        self.declare_parameter('scan_angle_min',  -math.pi)
+        self.scan_angle_min = \
+            self.get_parameter('scan_angle_min').get_parameter_value().double_value
+
+        self.declare_parameter('scan_angle_max',  math.pi)
+        self.scan_angle_max = \
+            self.get_parameter('scan_angle_max').get_parameter_value().double_value
+
+        self.declare_parameter('scan_angle_increment',  0.0261)  # 0.0261 rad = 1.5 degrees
+        self.scan_angle_increment = \
+            self.get_parameter('scan_angle_increment').get_parameter_value().double_value
+
+        self.declare_parameter('scan_use_inf', True)
+        self.use_inf = \
+            self.get_parameter('scan_use_inf').get_parameter_value().bool_value
+
         self.t_map_to_odom = TransformStamped()
         self.t_map_to_odom.header.frame_id = self.map_frame_id
         self.t_map_to_odom.child_frame_id = self.odom_frame_id
@@ -235,14 +259,14 @@ def publishLaserScan(self) -> None:
         self.scan_msg = LaserScan()
         self.scan_msg.header.stamp = (self.get_clock().now()).to_msg()
         self.scan_msg.header.frame_id = self.scan_frame_id
-        self.scan_msg.angle_min = -math.pi
-        self.scan_msg.angle_max = math.pi
+        self.scan_msg.angle_min = self.scan_angle_min
+        self.scan_msg.angle_max = self.scan_angle_max
         # 1.5 degrees
-        self.scan_msg.angle_increment = 0.0261799
+        self.scan_msg.angle_increment = self.scan_angle_increment
         self.scan_msg.time_increment = 0.0
         self.scan_msg.scan_time = 0.1
-        self.scan_msg.range_min = 0.05
-        self.scan_msg.range_max = 30.0
+        self.scan_msg.range_min = self.scan_range_min
+        self.scan_msg.range_max = self.scan_range_max
         num_samples = int(
             (self.scan_msg.angle_max - self.scan_msg.angle_min) /
             self.scan_msg.angle_increment)
@@ -323,7 +347,10 @@ def getLaserPose(self) -> tuple[float, float, float]:
 
     def getLaserScan(self, num_samples: int) -> None:
         if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None:
-            self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
+            if self.use_inf:
+                self.scan_msg.ranges = [float('inf')] * num_samples
+            else:
+                self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
             return
 
         x0, y0, theta = self.getLaserPose()
@@ -332,7 +359,10 @@ def getLaserScan(self, num_samples: int) -> None:
 
         if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height:
             # outside map
-            self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
+            if self.use_inf:
+                self.scan_msg.ranges = [float('inf')] * num_samples
+            else:
+                self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
             return
 
         for i in range(num_samples):
@@ -361,6 +391,8 @@ def getLaserScan(self, num_samples: int) -> None:
                     break
 
                 line_iterator.advance()
+            if self.scan_msg.ranges[i] == 0.0 and self.use_inf:
+                self.scan_msg.ranges[i] = float('inf')
 
 
 def main() -> None: