"ROS2 Nav2 Theta* Planner Resolution Dependency Issue"

[MehmetKUTAN]

Bug report

Required Info:

• Operating System:
  • Ubuntu Jammy 22.04
• ROS2 Version:
  • Humble
• Version or commit hash:
  • humble- Nav2-theta star planner

Steps to reproduce issue

Step 1: Environment and Preparation

  Set up your ROS 2 system and install the Nav2 packages.
  Prepare the necessary map and configuration files.

Step 2: Creating a Workspace

  Create or activate your ROS 2 workspace for working with Nav2.
  Set up a workspace for working with Nav2.

Step 3: Map and Configuration Files

Edit your map and configuration files to change the resolution value to 1.5 cm and 1 cm.

Step 4: Launching Theta Star Planner

>   Start your ROS 2 system and run the required commands to launch Theta Star Planner with Nav2.

Step 5: Reproducing the Issue

> Choose the same start and target points for both the high-resolution (1.5 cm) and low-resolution (1 cm) versions of the map.
>     First, launch Theta Star Planner with the high-resolution map and record the resulting planned path.
>     Then, launch Theta Star Planner with the low-resolution map and record the resulting planned path.

Step 6: Comparing the Results

>   Compare the planned paths obtained for two different resolution levels.
>           Validate your issue by comparing the path generated in the high-resolution map with the path generated in the low-resolution map for a specific start and target point.

Step 7: Generating a Report

 Document your findings and create a detailed report.
  In your report, explain the configuration of the map files used, how Theta Star Planner was launched, and how the issue was reproduced.
  Additionally, in your report, describe the difficulties and results related to the resolution value.

Expected Behavior:

• When using the ROS2 Nav2 Theta Star Planner with a map of a given resolution (e.g., 1.5 cm), the planner should correctly identify obstacles in the environment and generate a path that navigates around those obstacles.
• The generated path should adhere to the map's resolution and avoid obstacles, providing a clear and efficient route from the starting point to the target destination.
• The planner should demonstrate consistent and reliable behavior, producing similar results for the same map and configuration under similar circumstances.

Actual behavior

• When using ROS2 Nav2 Theta Star Planner with a map of resolution 1 cm, the planner encounters difficulty in correctly identifying obstacles in the environment. It often generates a path that goes over obstacles rather than around them.
• The generated path may not align with the map's resolution and can result in inefficient or problematic routes.
• The planner's behavior appears inconsistent when compared to its behavior with higher resolutions, where it correctly identifies and navigates around obstacles.

Additional information

ROS 2 Version: ROS 2 "Humble" version.

Theta Star Planner Settings:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: false
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_theta_star_planner/ThetaStarPlanner"
      how_many_corners: 8
      w_euc_cost: 1.0
      w_traversal_cost: 1.70
    w_heuristic_cost: 1.0

Types of Obstacles: Local costmap (static or dynamic).

Other Resolution Values: 1 cm and lower.

Resolution: 1 cm

001.mp4

Resolution: 1.5 cm

0015.mp4

[SteveMacenski]

A couple of follow up questions:

• Turning off the local costmap, do you see that your global costmap correctly has the obstacles? The local costmap on top is blocking a view of that
• Do you see this behavior when you work with lower resolution maps that are more standard, 3-5cm?
• Do you see this behavior with your higher resolution maps with the NavFn planner or Smac2D planners?

These will help me narrow down where to look or provide suggestions. By the way, those map resolutions seem kind of crazy fine at 1mm. Do you have sensing that can even generate maps that fine accurately? Either way, I want to see what the root cause of this issue is to resolve it, but the settings you're using are quite strange and perhaps are worth reexamining.

Do you have something I can reproduce?

[MehmetKUTAN]

I apologize for the earlier message. I provided incorrect information. The map resolution is actually set at 1 cm and 1.5 cm, not 1 mm. I have corrected this misinformation. Thank you for your attention and understanding

1. Turning off the local costmap, do you see that your global costmap correctly has the obstacles? The local costmap on top is blocking a view of that

Resolution: 1 cm
Planner: ThetaStar
Video:

global-thetastar.mp4

2. Do you see this behavior when you work with lower resolution maps that are more standard, 3-5cm?

Resolution: 3.5 cm
Planner: ThetaStar
Video:

0035.mp4

3. Do you see this behavior with your higher resolution maps with the NavFn planner or Smac2D planners?

Resolution: 1 cm
Planner: NavFn Planner
Video:

Navfn_001.mp4

[SteveMacenski]

To be clear from these videos, this only happens at very low resolution Theta*, correct? Some detail from your experiments might help...

I won't have some time look into this for you in detail, but https://github.com/ros-planning/navigation2/blob/main/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp#L191-L202 this is where I would start to look in conjunction with the line of sight checks https://github.com/ros-planning/navigation2/blob/main/nav2_theta_star_planner/src/theta_star.cpp. Something must be a little off - the overall planner is pretty short / simple so there's only so many potential locations for errors.

I'd appreciate if you took a look and let me know what you think / find!

Edit: oh actually, what if you put

  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));

here in the code? This rhymes with something else I was helping another user with a couple of weeks ago

[MehmetKUTAN]

I used this code snippet in the specified location and the problem was solved .I pasted the code here

`std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(planner_->costmap_->getMutex()));`

video: map is 1 cm

nav2-thetastar.mp4

[SteveMacenski]

#3918 to resolve, thanks for the report!