diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 8eb82040d..f59ffbb41 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -3,6 +3,8 @@
+
+
@@ -208,6 +210,92 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/launch/sim/localization_launch.py b/src/hangar_sim/launch/sim/localization_launch.py
index 6d8245f69..dcccccf2f 100644
--- a/src/hangar_sim/launch/sim/localization_launch.py
+++ b/src/hangar_sim/launch/sim/localization_launch.py
@@ -56,7 +56,7 @@ def generate_launch_description():
use_respawn = LaunchConfiguration("use_respawn")
log_level = LaunchConfiguration("log_level")
- lifecycle_nodes = ["map_server", "amcl"]
+ lifecycle_nodes = ["map_server"]
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -181,13 +181,6 @@ def generate_launch_description():
parameters=[configured_params],
remappings=remappings,
),
- ComposableNode(
- package="nav2_amcl",
- plugin="nav2_amcl::AmclNode",
- name="amcl",
- parameters=[configured_params],
- remappings=remappings,
- ),
ComposableNode(
package="nav2_lifecycle_manager",
plugin="nav2_lifecycle_manager::LifecycleManager",
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
index 92d81f6c8..848672db8 100644
--- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
+++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -284,8 +284,25 @@ def generate_launch_description():
output="log",
)
- # Fuse state estimator for mobile base localization
hangar_sim_pkg = FindPackageShare("hangar_sim")
+
+ # Angular bounds filter: clips chassis self-hitting beams (±93° to ±135°),
+ # publishes clean scan to /scan_filtered for Nav2 costmap.
+ laser_filter_node = Node(
+ package="laser_filters",
+ executable="scan_to_scan_filter_chain",
+ name="laser_angular_filter",
+ parameters=[
+ PathJoinSubstitution(
+ [hangar_sim_pkg, "params", "laser_filter_params.yaml"]
+ ),
+ {"use_sim_time": use_sim_time},
+ {"qos_overrides./scan.subscription.reliability": "best_effort"},
+ ],
+ output="log",
+ )
+
+ # Fuse state estimator for mobile base localization
fuse_state_estimator = Node(
package="fuse_optimizers",
executable="fixed_lag_smoother_node",
@@ -326,6 +343,7 @@ def generate_launch_description():
ld.add_action(static_tf_map_to_odom)
ld.add_action(odom_to_joint_state_repub)
ld.add_action(odom_qos_relay)
+ ld.add_action(laser_filter_node)
ld.add_action(fuse_state_estimator)
return ld
diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml
index f06c465f3..9eb2a1e65 100644
--- a/src/hangar_sim/package.xml
+++ b/src/hangar_sim/package.xml
@@ -19,6 +19,7 @@
moveit_ros_perception
moveit_studio_agent
moveit_pro_behavior
+ laser_filters
nav2_bringup
picknik_accessories
picknik_mujoco_ros
diff --git a/src/hangar_sim/params/laser_filter_params.yaml b/src/hangar_sim/params/laser_filter_params.yaml
new file mode 100644
index 000000000..448340813
--- /dev/null
+++ b/src/hangar_sim/params/laser_filter_params.yaml
@@ -0,0 +1,20 @@
+# Angular bounds filter for the dual SICK TIM571 lidars.
+#
+# Both lidars publish to /scan with angle_min=0 and angle_max=4.7124 (270 deg sweep).
+# The lidar _ROS frame X axis points at beam-0, so scan angle alpha maps to robot frame
+# angle (alpha + frame_yaw):
+# 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.
+laser_angular_filter:
+ ros__parameters:
+ filter1:
+ name: angular_bounds
+ type: laser_filters/LaserScanAngularBoundsFilter
+ params:
+ lower_angle: 0.7854
+ upper_angle: 3.9270
diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml
index 2097ff000..fbac623cb 100644
--- a/src/hangar_sim/params/nav2_params.yaml
+++ b/src/hangar_sim/params/nav2_params.yaml
@@ -126,7 +126,7 @@ controller_server:
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
- movement_time_allowance: 10.0
+ movement_time_allowance: 20.0
# Goal checker parameters
#precise_goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
@@ -142,7 +142,7 @@ controller_server:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
- batch_size: 2000
+ batch_size: 500
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
@@ -235,47 +235,42 @@ controller_server:
local_costmap:
local_costmap:
ros__parameters:
- update_frequency: 5.0
- publish_frequency: 2.0
+ update_frequency: 10.0
+ publish_frequency: 5.0
global_frame: odom
robot_base_frame: ridgeback_base_link
use_sim_time: True
rolling_window: true
- width: 3
- height: 3
+ width: 10
+ height: 10
resolution: 0.05
footprint: "[[0.45, 0.375], [0.45, -0.375], [-0.45, -0.375], [-0.45, 0.375]]"
footprint_padding: 0.025
- plugins: ["voxel_layer", "inflation_layer"]
+ plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
- cost_scaling_factor: 1.50
- inflation_radius: 2.2
- voxel_layer:
- plugin: "nav2_costmap_2d::VoxelLayer"
+ cost_scaling_factor: 3.0
+ inflation_radius: 0.75
+ obstacle_layer:
+ plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
- publish_voxel_map: True
- origin_z: 0.0
- z_resolution: 0.1
- z_voxels: 16
- max_obstacle_height: 3.0
- mark_threshold: 1
- observation_sources: pointcloud
- pointcloud:
- topic: /left_camera/points
- max_obstacle_height: 3.0
+ observation_sources: scan
+ scan:
+ topic: /scan_filtered
+ max_obstacle_height: 2.0
clearing: True
marking: True
- raytrace_max_range: 3.0
+ data_type: "LaserScan"
+ raytrace_max_range: 10.0
raytrace_min_range: 0.0
- obstacle_max_range: 2.5
- obstacle_min_range: 0.0
- data_type: "PointCloud2"
- static_layer:
- plugin: "nav2_costmap_2d::StaticLayer"
- map_subscribe_transient_local: True
+ obstacle_max_range: 8.0
+ inf_is_valid: false
+ qos_overriding_options:
+ policy_kinds: [reliability]
+ reliability: best_effort
always_send_full_costmap: True
+
global_costmap:
global_costmap:
ros__parameters:
@@ -288,34 +283,32 @@ global_costmap:
footprint_padding: 0.025
resolution: 0.05
track_unknown_space: true
- plugins: ["static_layer", "voxel_layer", "inflation_layer"]
- voxel_layer:
- plugin: "nav2_costmap_2d::VoxelLayer"
+ plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ obstacle_layer:
+ plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
- publish_voxel_map: True
- origin_z: 0.1
- z_resolution: 0.0
- z_voxels: 16
- max_obstacle_height: 3.0
- mark_threshold: 2
- observation_sources: pointcloud
- pointcloud:
- topic: /left_camera/points
- max_obstacle_height: 3.0
+ observation_sources: scan
+ scan:
+ topic: /scan_filtered
+ max_obstacle_height: 2.0
clearing: True
marking: True
+ data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
- data_type: "PointCloud2"
- static_layer:
- plugin: "nav2_costmap_2d::StaticLayer"
- map_subscribe_transient_local: True
+ inf_is_valid: false
+ qos_overriding_options:
+ policy_kinds: [reliability]
+ reliability: best_effort
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
- cost_scaling_factor: 1.50
- inflation_radius: 2.20
+ cost_scaling_factor: 4.0
+ inflation_radius: 0.7
always_send_full_costmap: True
map_server: