diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index d9cf87e5cb0..bf148f60b3d 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index fc160d91070..62104e05e9b 100644 --- a/nav2_loopback_sim/README.md +++ b/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 diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 53d67540214..77a69143a9c 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/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: