From 8387b4ccc5184658365936e83426762e02bd2be0 Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Thu, 21 May 2026 21:49:16 -0400 Subject: [PATCH 1/2] fix(hangar_sim): switch lidar filter to LaserScanAngularBoundsFilterInPlace MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Filtered beams are now marked range_max instead of being removed, so the scan's angle_min/angle_max and beam count stay constant across messages. Nav2's inf_is_valid: false discards those beams correctly — same functional result as before, but with a stable scan structure that nav2 costmap expects. Co-Authored-By: Claude Sonnet 4.6 --- src/hangar_sim/params/laser_filter_params.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/hangar_sim/params/laser_filter_params.yaml b/src/hangar_sim/params/laser_filter_params.yaml index 484d7fb70..13af155de 100644 --- a/src/hangar_sim/params/laser_filter_params.yaml +++ b/src/hangar_sim/params/laser_filter_params.yaml @@ -14,11 +14,15 @@ # Two filter chain instances run, one per lidar, each remapped to its own # /scan_{front,rear} input and /scan_{front,rear}_filtered output. Nav2 consumes # both filtered topics as independent obstacle observation sources. +# +# InPlace variant is used so angle_min/angle_max and beam count stay constant across +# messages. Filtered beams are set to range_max (the "no return" sentinel); nav2's +# inf_is_valid: false setting discards them, which is the correct behaviour. laser_angular_filter_front: ros__parameters: filter1: name: angular_bounds - type: laser_filters/LaserScanAngularBoundsFilter + type: laser_filters/LaserScanAngularBoundsFilterInPlace params: lower_angle: 0.7854 upper_angle: 3.9270 @@ -27,7 +31,7 @@ laser_angular_filter_rear: ros__parameters: filter1: name: angular_bounds - type: laser_filters/LaserScanAngularBoundsFilter + type: laser_filters/LaserScanAngularBoundsFilterInPlace params: lower_angle: 0.7854 upper_angle: 3.9270 From 3b7e685e1fd30ee8561abd8fb36c24e880d3f916 Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Tue, 26 May 2026 11:29:09 -0400 Subject: [PATCH 2/2] fix(hangar_sim): switch lidar filter to InPlace and tighten angles Switches LaserScanAngularBoundsFilter to LaserScanAngularBoundsFilterInPlace so scan dimensions (angle_min, angle_max, beam count) stay constant across messages. Nav2 costmap plugins expect consistent scan structure; the existing inf_is_valid: false setting discards the range_max+1 sentinel beams. InPlace removes beams within [lower, upper] (not outside), so each filter chain uses two stages to remove the two rear-corner self-hit arcs separately. Empirical measurement from /scan_front raw data shows chassis self-hits only at scan 0-30 deg and 240-270 deg. Tightens the filter from 0-45 deg / 225-270 deg (90 deg total, ~30 beams) to 0-30 deg / 240-270 deg (60 deg total, ~21 beams), preserving valid obstacle returns from scan 33-237 deg per lidar. Also adds mujoco_viewer: false to config.yaml to document the default. Co-Authored-By: Claude Sonnet 4.6 --- src/hangar_sim/config/config.yaml | 1 + .../params/laser_filter_params.yaml | 42 ++++++++++++++----- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml index 1b4157ccb..6162586fb 100644 --- a/src/hangar_sim/config/config.yaml +++ b/src/hangar_sim/config/config.yaml @@ -20,6 +20,7 @@ hardware: package: "hangar_sim" path: "config/moveit/joint_limits.yaml" - mujoco_model: "description/hangar_scene.xml" + - mujoco_viewer: false # set to true locally to open the MuJoCo viewer window # Set to false when use_fuse:=true so fuse is the sole odom -> ridgeback_base_link publisher. # Rebuild hangar_sim after changing this value. - publish_odom: true diff --git a/src/hangar_sim/params/laser_filter_params.yaml b/src/hangar_sim/params/laser_filter_params.yaml index 13af155de..f12c86aa1 100644 --- a/src/hangar_sim/params/laser_filter_params.yaml +++ b/src/hangar_sim/params/laser_filter_params.yaml @@ -6,32 +6,52 @@ # front lidar frame yaw = -135 deg: robot angle = scan_angle - 135 deg # rear lidar frame yaw = +45 deg: robot angle = scan_angle + 45 deg # -# Chassis self-hits occur when |robot_frame_angle| > 90 deg, i.e. beams looking more -# than 90 deg backward. This maps to scan angles 0-45 deg and 225-270 deg for both lidars. -# Keeping scan angles 45-225 deg (0.7854 to 3.9270 rad) eliminates all self-hits and -# gives each lidar a clean 180 deg arc; combined they cover 360 deg. +# Chassis self-hits occur in the rear corners of each lidar's sweep. Empirically +# (measured in simulation from /scan_front raw data) the self-hit beams are: +# scan 0-30 deg (0.000-0.524 rad): rear-corner hits at 0.25-0.28 m +# scan 240-270 deg (4.189-4.712 rad): rear-corner hits at 0.25-0.39 m +# Valid obstacle returns begin at scan ~33 deg. Filtering only these two narrow +# arcs removes all chassis self-hits while preserving the maximum useful coverage. +# Combined, each lidar keeps ~230 deg of valid arc; the two lidars together give +# full 360 deg obstacle coverage with only ~60 deg total filtered per lidar. # # Two filter chain instances run, one per lidar, each remapped to its own # /scan_{front,rear} input and /scan_{front,rear}_filtered output. Nav2 consumes # both filtered topics as independent obstacle observation sources. # # InPlace variant is used so angle_min/angle_max and beam count stay constant across -# messages. Filtered beams are set to range_max (the "no return" sentinel); nav2's +# messages. Filtered beams are set to range_max+1 (the "no return" sentinel); nav2's # inf_is_valid: false setting discards them, which is the correct behaviour. +# +# NOTE: LaserScanAngularBoundsFilterInPlace removes beams that fall WITHIN [lower, upper], +# not outside. Two filter stages per chain are therefore needed to remove the two rear +# corner arcs without touching the valid forward arc. laser_angular_filter_front: ros__parameters: filter1: - name: angular_bounds + name: remove_rear_right + type: laser_filters/LaserScanAngularBoundsFilterInPlace + params: + lower_angle: 0.0 + upper_angle: 0.5236 + filter2: + name: remove_rear_left type: laser_filters/LaserScanAngularBoundsFilterInPlace params: - lower_angle: 0.7854 - upper_angle: 3.9270 + lower_angle: 4.1888 + upper_angle: 4.7124 laser_angular_filter_rear: ros__parameters: filter1: - name: angular_bounds + name: remove_rear_right + type: laser_filters/LaserScanAngularBoundsFilterInPlace + params: + lower_angle: 0.0 + upper_angle: 0.5236 + filter2: + name: remove_rear_left type: laser_filters/LaserScanAngularBoundsFilterInPlace params: - lower_angle: 0.7854 - upper_angle: 3.9270 + lower_angle: 4.1888 + upper_angle: 4.7124