Skip to content

Commit efaa132

Browse files
griswaldbrooksgriswaldbrooks
committed
17875 Add navigate-with-replanning objective and dual-lidar nav2 integration for ur5e_ridgeback
1 parent a3bf7b1 commit efaa132

5 files changed

Lines changed: 147 additions & 22 deletions

File tree

‎src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,16 @@
33
<xacro:macro name="picknik_ur_mujoco_ros2_control" params="mujoco_model mujoco_viewer:=false">
44

55
<ros2_control name="ur_mujoco_control" type="system">
6+
<!-- Per-LiDAR topic routing. Sensor names match the MJCF site-name prefix
7+
(the portion before the '-' in replicated rangefinder site names).
8+
Each LiDAR publishes to its own topic so Nav2's obstacle layer can
9+
treat them as independent observation sources. -->
10+
<sensor name="lidar_front">
11+
<param name="topic">/scan_front</param>
12+
</sensor>
13+
<sensor name="lidar_rear">
14+
<param name="topic">/scan_rear</param>
15+
</sensor>
616
<hardware>
717
<plugin>picknik_mujoco_ros/MujocoSystem</plugin>
818
<param name="mujoco_model">${mujoco_model}</param>

‎src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py‎

Lines changed: 33 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -284,16 +284,40 @@ def generate_launch_description():
284284

285285
hangar_sim_pkg = FindPackageShare("hangar_sim")
286286

287-
# Angular bounds filter: clips chassis self-hitting beams (±93° to ±135°),
288-
# publishes clean scan to /scan_filtered for Nav2 costmap.
289-
laser_filter_node = Node(
287+
# Angular bounds filter: clips chassis self-hitting beams (±93° to ±135°).
288+
# One filter instance per lidar; each publishes its filtered scan to
289+
# /scan_{front,rear}_filtered for Nav2 to consume as an independent
290+
# obstacle observation source.
291+
laser_filter_params = PathJoinSubstitution(
292+
[hangar_sim_pkg, "params", "laser_filter_params.yaml"]
293+
)
294+
295+
laser_filter_front_node = Node(
290296
package="laser_filters",
291297
executable="scan_to_scan_filter_chain",
292-
name="laser_angular_filter",
298+
name="laser_angular_filter_front",
299+
remappings=[
300+
("scan", "/scan_front"),
301+
("scan_filtered", "/scan_front_filtered"),
302+
],
293303
parameters=[
294-
PathJoinSubstitution(
295-
[hangar_sim_pkg, "params", "laser_filter_params.yaml"]
296-
),
304+
laser_filter_params,
305+
{"use_sim_time": use_sim_time},
306+
{"qos_overrides./scan.subscription.reliability": "best_effort"},
307+
],
308+
output="log",
309+
)
310+
311+
laser_filter_rear_node = Node(
312+
package="laser_filters",
313+
executable="scan_to_scan_filter_chain",
314+
name="laser_angular_filter_rear",
315+
remappings=[
316+
("scan", "/scan_rear"),
317+
("scan_filtered", "/scan_rear_filtered"),
318+
],
319+
parameters=[
320+
laser_filter_params,
297321
{"use_sim_time": use_sim_time},
298322
{"qos_overrides./scan.subscription.reliability": "best_effort"},
299323
],
@@ -342,7 +366,8 @@ def generate_launch_description():
342366
ld.add_action(static_tf_world_to_map)
343367
ld.add_action(static_tf_map_to_odom)
344368
ld.add_action(sensor_qos_relay)
345-
ld.add_action(laser_filter_node)
369+
ld.add_action(laser_filter_front_node)
370+
ld.add_action(laser_filter_rear_node)
346371
ld.add_action(fuse_state_estimator)
347372

348373
return ld

‎src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="Navigate to Clicked Point with Replanning"
5+
>
6+
<!--//////////-->
7+
<BehaviorTree
8+
ID="Navigate to Clicked Point with Replanning"
9+
_description="Navigate to a point clicked by the user in the UI. This Objective will repeatedly replan the path as new obstacles appear on the local costmap."
10+
_favorite="true"
11+
>
12+
<Control ID="Sequence" name="TopLevelSequence">
13+
<Action
14+
ID="CreateStampedPose"
15+
reference_frame="base_link"
16+
stamped_pose="{start}"
17+
/>
18+
<Action
19+
ID="SwitchController"
20+
activate_controllers="platform_velocity_controller_nav2"
21+
deactivate_controllers="velocity_force_controller;joint_trajectory_controller;platform_velocity_controller"
22+
/>
23+
<Action
24+
ID="GetPoseFromUser"
25+
is_normal="true"
26+
pose_prompt="Click point on UI"
27+
user_pose="{goal}"
28+
view_name="Visualization"
29+
/>
30+
<Action
31+
ID="NavigateToPoseAction"
32+
action_name="/navigate_to_pose"
33+
behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
34+
pose_stamped="{goal}"
35+
/>
36+
<Action
37+
ID="SwitchController"
38+
activate_controllers="joint_trajectory_controller"
39+
/>
40+
</Control>
41+
</BehaviorTree>
42+
<TreeNodesModel>
43+
<SubTree ID="Navigate to Clicked Point with Replanning">
44+
<MetadataFields>
45+
<Metadata runnable="true" />
46+
<Metadata subcategory="Navigation" />
47+
</MetadataFields>
48+
</SubTree>
49+
</TreeNodesModel>
50+
</root>

‎src/hangar_sim/params/laser_filter_params.yaml‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,29 @@
11
# Angular bounds filter for the dual SICK TIM571 lidars.
22
#
3-
# Both lidars publish to /scan with angle_min=0 and angle_max=4.7124 (270 deg sweep).
4-
# The lidar _ROS frame X axis points at beam-0, so scan angle alpha maps to robot frame
5-
# angle (alpha + frame_yaw):
3+
# Each lidar publishes to its own topic (/scan_front, /scan_rear) with angle_min=0
4+
# and angle_max=4.7124 (270 deg sweep). The lidar _ROS frame X axis points at beam-0,
5+
# so scan angle alpha maps to robot frame angle (alpha + frame_yaw):
66
# front lidar frame yaw = -135 deg: robot angle = scan_angle - 135 deg
77
# rear lidar frame yaw = +45 deg: robot angle = scan_angle + 45 deg
88
#
99
# Chassis self-hits occur when |robot_frame_angle| > 90 deg, i.e. beams looking more
1010
# than 90 deg backward. This maps to scan angles 0-45 deg and 225-270 deg for both lidars.
1111
# Keeping scan angles 45-225 deg (0.7854 to 3.9270 rad) eliminates all self-hits and
1212
# gives each lidar a clean 180 deg arc; combined they cover 360 deg.
13-
laser_angular_filter:
13+
#
14+
# Two filter chain instances run, one per lidar, each remapped to its own
15+
# /scan_{front,rear} input and /scan_{front,rear}_filtered output. Nav2 consumes
16+
# both filtered topics as independent obstacle observation sources.
17+
laser_angular_filter_front:
18+
ros__parameters:
19+
filter1:
20+
name: angular_bounds
21+
type: laser_filters/LaserScanAngularBoundsFilter
22+
params:
23+
lower_angle: 0.7854
24+
upper_angle: 3.9270
25+
26+
laser_angular_filter_rear:
1427
ros__parameters:
1528
filter1:
1629
name: angular_bounds

‎src/hangar_sim/params/nav2_params.yaml‎

Lines changed: 37 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -254,16 +254,29 @@ local_costmap:
254254
obstacle_layer:
255255
plugin: "nav2_costmap_2d::ObstacleLayer"
256256
enabled: True
257-
observation_sources: scan
258-
scan:
259-
topic: /scan_filtered
257+
observation_sources: scan_front scan_rear
258+
scan_front:
259+
topic: /scan_front_filtered
260260
max_obstacle_height: 2.0
261261
clearing: True
262262
marking: True
263263
data_type: "LaserScan"
264-
raytrace_max_range: 10.0
264+
raytrace_max_range: 8.0
265265
raytrace_min_range: 0.0
266-
obstacle_max_range: 8.0
266+
obstacle_max_range: 7.0
267+
inf_is_valid: false
268+
qos_overriding_options:
269+
policy_kinds: [reliability]
270+
reliability: best_effort
271+
scan_rear:
272+
topic: /scan_rear_filtered
273+
max_obstacle_height: 2.0
274+
clearing: True
275+
marking: True
276+
data_type: "LaserScan"
277+
raytrace_max_range: 8.0
278+
raytrace_min_range: 0.0
279+
obstacle_max_range: 7.0
267280
inf_is_valid: false
268281
qos_overriding_options:
269282
policy_kinds: [reliability]
@@ -290,16 +303,30 @@ global_costmap:
290303
obstacle_layer:
291304
plugin: "nav2_costmap_2d::ObstacleLayer"
292305
enabled: True
293-
observation_sources: scan
294-
scan:
295-
topic: /scan_filtered
306+
observation_sources: scan_front scan_rear
307+
scan_front:
308+
topic: /scan_front_filtered
309+
max_obstacle_height: 2.0
310+
clearing: True
311+
marking: True
312+
data_type: "LaserScan"
313+
raytrace_max_range: 18.0
314+
raytrace_min_range: 0.0
315+
obstacle_max_range: 15.0
316+
obstacle_min_range: 0.0
317+
inf_is_valid: false
318+
qos_overriding_options:
319+
policy_kinds: [reliability]
320+
reliability: best_effort
321+
scan_rear:
322+
topic: /scan_rear_filtered
296323
max_obstacle_height: 2.0
297324
clearing: True
298325
marking: True
299326
data_type: "LaserScan"
300-
raytrace_max_range: 3.0
327+
raytrace_max_range: 18.0
301328
raytrace_min_range: 0.0
302-
obstacle_max_range: 2.5
329+
obstacle_max_range: 15.0
303330
obstacle_min_range: 0.0
304331
inf_is_valid: false
305332
qos_overriding_options:

0 commit comments

Comments
 (0)