DWA planner failed to produce path.

[kellen080]

Hello, matlabbe

Thank you for both the RTAB-Map and this drone project.

After several testing, I found that when in localization mode, the drone can not avoid unknown obstacle which is not appear in 2D occupancy grid map(means that the obstacle is not appear in mapping mode).

I think the goal_cost probably is < 0 and it kept appear this information repeatedly until rotate_recovery behavior.

[WARN] DWA planner failed to produce path.
[DEBUG] The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.

But when in mapping mode, the drone can avoid obstacles. Maybe this is global planner working, because 2D occupancy map is changing, than global path changes afterward to avoid obstacle.

I'm not sure is local planner working?

I referring some articles 1 2 to dynamically turn some parameters like

base_local_planner_params.yaml

sim_time
path_distance_bias
goal_distance_bias
occdist_scale

costmap_common_params.yaml

footprint
robot_radius
inflation_radius

local_costmap_params.yaml

width
height

But, it seems doesn't work as well.

I apologize for bothering you. Thank you in advance.

[matlabbe]

I reproduced the issue. The problem is that DWA tries too much to follow the global path (which computed without obstacle). On a ground robot, I would say add an obstacle layer plugin (e.g., here the voxel grid) to global costmap config:

  plugins:
    - {name: static_layer,    type: "rtabmap_costmap_plugins::StaticLayer"}
    - {name: voxel_layer,     type: "rtabmap_costmap_plugins::VoxelLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

so that the global path is computed by including the obstacle. However in this example it seems we cannot use our modified voxel grid in the global costmap. As soon as the drone takes off:

The origin for the sensor at (-0.02, -3.26, 1.49) is out of map bounds. So, the costmap cannot raytrace for it.

As a workaround, on rtabmap side we can assemble the latest sensor reading to static map. To do so, add the following to slam.launch:

<param name="/rtabmap/rtabmap/map_always_update" value="true"/>

PS: by the way I just updated this repo to be compatible with latest rtabmap_ros.

[kellen080]

Hello, matlabbe

Thank you for answering this issue.

After I adjust path_distance_bias to 0.1, the local planner can avoid obstacle.

And I try both of these two answer individually:

- {name: voxel_layer,     type: "rtabmap_costmap_plugins::VoxelLayer"}

 <param name="/rtabmap/rtabmap/map_always_update" value="true"/>

---

First

 - {name: voxel_layer,     type: "rtabmap_costmap_plugins::VoxelLayer"}

The warning you said does not appear but drone still can not avoid obstacle. It seems nothing happen after I add this line to global_costmap_params.yaml.

---

Second

<param name="/rtabmap/rtabmap/map_always_update" value="true"/>

Although drone can avoid obstacle, 2d occupancy map can not change (from occupied grid to free space grid) after I move the obstacle out . By doing so, the drone can not fly through the free space.

Do you have this kind of problem ?

Best Regards Kellen

[matlabbe]

To add VoxelLayer to global costmap, you have to setup parameters like I did in the local costmap params file. However, it seems not working anyway.

To clear obstacles, you would need to enable ray tracing. To do so, add --Grid/RayTracing True in args of included rtabmap.launch in slam.launch. I did try it and it should work.

cheers, Mathieu

[kellen080]

Hello, Mathieu

I did some change as you said but it still not working, but it's fine.

Method 1

global_costmap_params.yaml.

- {name: voxel_layer,     type: "rtabmap_costmap_plugins::VoxelLayer"}

Method 2

slam.launch

<param name="/rtabmap/rtabmap/map_always_update" value="true"/>

config.ini

--Grid/RayTracing True

---

After several parameter adjustments and failed experiments, now it can fly stably with obstacle avoidance in both virtual environment and real environment. Also, I have tested different conditions including unknown static obstacle and unknown dynamic obstacle. Thank you very much.

Best Regards Kellen

[matlabbe]

I did some change as you said but it still not working

Did method 2 with Grid/RayTracing work? For clearing dynamic obstacles in the global static map, Grid/RayTracing should be true. Not sure about the config.ini, is this file included by rtabmap node (with config_path param)? it would not be --Grid/RayTracing True inside the config file, but Grid\RayTracing=true under [Core] section

[kellen080]

Hello, Mathieu

Yes, it's Grid\RayTracing=true in rtabmap cfg: config.ini file.

By the way, the config.ini is not the same as described in http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning

which says it will generated automatically. It's not.

---

Part 1. Obstacle Avoidance with Global Planning

After recheck the config.ini, I only modified real world config file, not simulation config file.

It's working now, I record a short video below.

Demo Video

https://github.com/matlabbe/rtabmap_drone_example/assets/48167189/f90d03ff-42ad-4f14-85bb-55df3478fd76

---

Part 2. New Question about Local Planning

I have another question about the local planner (here not using map_always_update).

I found a phenomenon that if obstacle outside the range of dwa cost(which 6x6 here) while giving a goal and global path go through the obstacle.

The local planner will failed and start recovery behavior (circle around) after it sense the obstacle when uav keeps going ahead.

But, if I make obstacle inside the range of dwa cost before giving goal.

The local planner can avoid obstacle smoothly.

Check two cases, demo videos below.

Planning Success

https://github.com/matlabbe/rtabmap_drone_example/assets/48167189/6f4f1c5e-5751-4a96-b4b5-31bf5ed5d205

Planning Fail

https://github.com/matlabbe/rtabmap_drone_example/assets/48167189/56d0da4e-0c4f-4712-a2cb-9f8d606eeea1

Maybe there's something I missing?

Thank you very much to answer my question.

Best Regards Kellen

[matlabbe]

1) thx to point out, I revised the doc http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning#Change_Parameters

The original config file is actually saved under ~/.rtabmap/rtabmap.ini after launching rtabmap standalone for the first time.

2) For the planning, that's a good question. I am not sure I can answer it, it looks like DWA related. The planning success example is what I would expect from the local planner. Is your sim time high? maybe similar question https://answers.ros.org/question/361386/navigation-stack-dwa-local-planner-crashes-into-dynamic-obstacles/

[kellen080]

Hello, Mathieu

After checking the second reference you pose, I think probably not parameters problem. Not sure anyway. My sim_time set to 4, it looks good. Don't have any idea for now. It's weird.

Thank you for answer this issue and propose another method (map_always_update). Just don't know what the keyword is for this kind of problem to search the answer.

Best Regards Kellen