Add replanning objective and dual-lidar nav2 integration for ur5e_ridgeback

[bgill92]

Summary

• Adds navigate_to_clicked_point_with_replanning.xml objective for ur5e_ridgeback in hangar_sim.
• Objective activates platform_velocity_controller_nav2 at entry and switches back to joint_trajectory_controller at exit, so whole-body planning is the default state after navigation completes (matches the pattern in the sibling navigate_to_clicked_point.xml).
• Enables both front and rear SICK TIM571 lidars on ur5e_ridgeback, routing each to a per-sensor Nav2 topic (/scan_front, /scan_rear) via the <sensor>-block feature added in PickNikRobotics/moveit_pro#18160.
• Two parallel scan_to_scan_filter_chain instances apply LaserScanAngularBoundsFilter (0.7854–3.9270 rad) to each scan independently, clipping chassis self-hits. Combined arcs give 360° coverage with no self-return overlap.
• Nav2 obstacle layer (local + global costmaps) uses scan_front scan_rear as observation sources.
• Resized the local costmap rolling window from 10 × 10 m to 5 × 5 m (2.5 m axial, ~3.54 m diagonal).
• Tuned obstacle / raytrace ranges for the TIM571:
  • Local: obstacle_max_range: 3.5, raytrace_max_range: 4.5 — matches the 5 × 5 m rolling window diagonal with a 1 m clearing margin.
  • Global: obstacle_max_range: 8.0, raytrace_max_range: 10.0 — keeps the global planner's horizon comfortably beyond the local window while staying within the sensor's high-confidence range.

Dependencies

Requires PickNikRobotics/moveit_pro#18160 (per-sensor MuJoCo LiDAR topic configuration). Must merge first.

Test plan

• Both /scan_front and /scan_rear publish from MuJoCo.
• /scan_front_filtered and /scan_rear_filtered combine to cover 360° with no chassis artifacts.
• Nav2 marks obstacles from both lidars around the full robot footprint in both costmaps.
• Global planner uses the global costmap — verified by temporarily disabling the local obstacle_layer and observing that the plan still routes around lidar-detected obstacles that aren't in the static map.
• Rear-lidar-only run verifies the rear pipeline (MJCF → /scan_rear → filter → Nav2) works independently.

[bkanator]

interesting, so I verified that it is working.
A couple notes though 1) it does not confirm the path, which may be a nice to have. 2) it does not show the re-plans or path it is about to take ? and on my first attempt I 3) got it into an infinite re-plan loop

I took a video of the first try and just luckily picked a final goal it can never get to! ha. So it is definitely re-planning!

so that leads to 4) should we somehow realize the goal is unachievable and notify the user, or should they be smart enough to just hit stop objective like I did. Watch below

replan-2026-04-02_15.56.34.mp4

[griswaldbrooks]

Agree with @bkanator on 1.
For 2, this ticket https://github.com/PickNikRobotics/moveit_pro/issues/17813 is supposed to do that. On 3, @bgill92 should this be handled by configuring the timeout on NavigateToPoseAction or can the recovery server be configured somehow? For me, timeouts don't always make sense as something close to the robot and ones far away will take different times... though... I'm not sure the robot ever failed, it just kept trying

[griswaldbrooks]

https://docs.nav2.org/behavior_trees/trees/nav_to_pose_recovery.html says it should try 6 times, but doesn't say what constitutes failure without reading the tree

[bgill92]

@bkanator @griswaldbrooks

1. Right now with NavigateToPoseAction, there is no mechanism to extract the path or tell it to wait for initial path confirmation. The Behavior sends the pose goal to the /navigate_to_pose action server and it automatically starts to navigate. I could add a VisualizePose and maybe a breakpoint or something? But the path cannot be visualized until the frontend has been updated as stated by the ticket Griz linked
2. I think an alternate can be to increase the size of the inflation for the local costmap so that the robot doesn't get into those nooks and crannies in the local costmap that causes it to keep replanning.

[griswaldbrooks]

After discussion, we are going to block this pr on https://github.com/PickNikRobotics/moveit_pro/issues/17813 and https://github.com/PickNikRobotics/moveit_pro/issues/17504 as the behavior is a little unintuitive for the user considering retries and replanning without UI feedback.

[bkanator]

Should remember to switch controller back to jtc at the end of this objective so it defaults to whole body planning

[griswaldbrooks]

@bgill92 can you update this?
@mariopratts this is the right approach, yes?

[bkanator]

@bgill92 can you update this? @mariopratts this is the right approach, yes?

it is true that the objectives that need whole body do switch at the start (I believe also tele-op does this). I guess it's a question of what works via command line at the end of different objectives? unsure if we want a common state...... if we don't care about switching back then make a note to take that switch out of the end of the Navigate to Clicked Point objective as well. They should all work without it? so maybe it's just a thought exercise?

[griswaldbrooks]

Yeah, my question is about the switching back. Is that every objectives responsibility to switch when they are done or should every objective ensure they switch when they start? How do you know which controller you should end on?

[marioprats]

@mariopratts this is the right approach, yes?

Yes, I think that makes sense. An Objective that requires nav makes sure to switch to the nav2 controllers when it starts.

[griswaldbrooks]

And then switch back to JTC when it exits?