Skip to content

add lidar sensor and use obstacle_layer in costmap params #531

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
griswaldbrooks merged 1 commit into from
Mar 19, 2026
+176 −56
Merged

add lidar sensor and use obstacle_layer in costmap params #531

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
95 changes: 95 additions & 0 deletions src/hangar_sim/description/ur5e_ridgeback.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@

<option integrator="implicitfast" />

<size nuser_sensor="6" />

<default>
<default class="ur5e">
<material specular="0.5" shininess="0.25" />
Expand Down Expand Up @@ -208,6 +210,92 @@
<joint name="rotational_yaw_joint" type="hinge" axis="0 0 1" />
<body name="base_platform" pos="0 0 0" euler="0 0 0">
<body name="ridgeback_base_link">
<!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg in robot frame.
Quat rule: site Z axis = beam-0 firing direction. Site X axis must point in world -Z
so the plugin's Rot(-90,Y) produces a Z-up _ROS frame.
Formula: R_base = Rot_Z(phi) * Rot_Y(+90 deg), where phi = beam-0 robot-frame angle.
Front: phi=-135 deg -> R_base = Rot_Z(-135)*Rot_Y(90) -> quat=(0.2706,0.6533,0.2706,-0.6533).
angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
<body name="lidar_front_mount" pos="0.45 0 0.15">
<inertial
mass="0.25"
pos="0 0 0"
diaginertia="0.0001 0.0001 0.0001"
/>
<replicate count="91" sep="-" offset="0 0 0" euler="0 0 0.05236">
<site
name="lidar_front"
size="0.01"
pos="0 0 0"
quat="0.2706 0.6533 0.2706 -0.6533"
/>
</replicate>
<!-- TIM571 body: dark cylinder, visual-only -->
<geom
name="lidar_front_body"
type="cylinder"
size="0.030 0.042"
pos="0 0 0"
rgba="0.2 0.2 0.2 1"
contype="0"
conaffinity="0"
group="2"
/>
<!-- Scan plane: cyan disk showing sweep area -->
<geom
name="lidar_front_scan_plane"
type="cylinder"
size="0.30 0.002"
pos="0 0 0"
rgba="0 1 1 0.3"
contype="0"
conaffinity="0"
group="2"
/>
</body>

<!-- Rear SICK TIM571: mounted at x=-0.45m (rear bumper), z=0.15m.
91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg in robot frame.
Rear: phi=+45 deg -> R_base = Rot_Z(45)*Rot_Y(90) -> quat=(0.6533,-0.2706,0.6533,0.2706).
angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
<body name="lidar_rear_mount" pos="-0.45 0 0.15">
<inertial
mass="0.25"
pos="0 0 0"
diaginertia="0.0001 0.0001 0.0001"
/>
<replicate count="91" sep="-" offset="0 0 0" euler="0 0 0.05236">
<site
name="lidar_rear"
size="0.01"
pos="0 0 0"
quat="0.6533 -0.2706 0.6533 0.2706"
/>
</replicate>
<!-- TIM571 body: dark cylinder, visual-only -->
<geom
name="lidar_rear_body"
type="cylinder"
size="0.030 0.042"
pos="0 0 0"
rgba="0.2 0.2 0.2 1"
contype="0"
conaffinity="0"
group="2"
/>
<!-- Scan plane: magenta disk showing sweep area -->
<geom
name="lidar_rear_scan_plane"
type="cylinder"
size="0.30 0.002"
pos="0 0 0"
rgba="1 0 1 0.3"
contype="0"
conaffinity="0"
group="2"
/>
</body>
<geom
name="chassis_link"
mesh="chassis_link"
Expand Down Expand Up @@ -586,5 +674,12 @@
<framequat objtype="site" objname="imu_site" />
<gyro site="imu_site" />
<accelerometer site="imu_site" />
<!-- Front TIM571: 270 deg FOV, 3 deg resolution (91 beams), 0.05-25m range.
angle_min=0 so the _ROS frame X axis points at beam-0 (per plugin convention).
The sweep is 0 to 4.7124 rad (270 deg) in the sensor frame, which maps to
beam-0 at -135 deg and beam-90 at +135 deg in the robot frame. -->
<rangefinder site="lidar_front" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
<!-- Rear TIM571: same convention — angle_min=0, frame X points at beam-0 (+45 deg robot frame). -->
<rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
</sensor>
</mujoco>
9 changes: 1 addition & 8 deletions src/hangar_sim/launch/sim/localization_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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",
Expand Down
20 changes: 19 additions & 1 deletion src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down Expand Up @@ -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
1 change: 1 addition & 0 deletions src/hangar_sim/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<exec_depend>moveit_ros_perception</exec_depend>
<exec_depend>moveit_studio_agent</exec_depend>
<exec_depend>moveit_pro_behavior</exec_depend>
<exec_depend>laser_filters</exec_depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>picknik_accessories</exec_depend>
<exec_depend>picknik_mujoco_ros</exec_depend>
Expand Down
20 changes: 20 additions & 0 deletions src/hangar_sim/params/laser_filter_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
87 changes: 40 additions & 47 deletions src/hangar_sim/params/nav2_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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:
Expand Down
Loading