Implement allow_unknown feature

[MartinPeris]

Other planners offer the feature of being able to not allow searching for a path in unknown space.

Recently, I was playing around with this planner and found myself in need of this feature, so I added it. I am sharing it with the world in the hope of becoming rich and famous it being useful to others.

The default behavior stays unchanged, the robot will not fear the unknown. But if the parameter allow_unknown is set to false then unknown space will be ignored from the path search.

Cheers!

[mintar]

Thanks for your PR! I'm trying to verify that this is working, but for me it isn't. Steps to reproduce:

1. sudo apt install ros-$ROS_DISTRO-stage-ros

2. Add the following line to sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml:

      allow_unknown: false
   

3. roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch

4. Use RViz to set a goal somewhere in unknown space.

With your PR, SBPL shouldn't plan a path there, but it does.

[MartinPeris]

I am probably missing something, but in the steps to reproduce the behavior that you are getting, where is the code of this PR being compiled?

[mintar]

I forgot to mention that, but of course I first compiled the code of this PR. I also put a debug printout into the code, like this:

diff --git a/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml b/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
index 9f9b3dc..2cc4aae 100644
--- a/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
+++ b/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
@@ -4,3 +4,4 @@ SBPLLatticePlanner:
   allocated_time: 5.0
   initial_epsilon: 3.0
   forward_search: false
+  allow_unknown: false
diff --git a/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp b/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp
index 0e72aa3..632afe1 100644
--- a/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp
+++ b/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp
@@ -124,6 +124,7 @@ void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS*
     private_nh.param<int>("visualizer_skip_poses", visualizer_skip_poses_, 5);
 
     private_nh.param("allow_unknown", allow_unknown_, bool(true));
+    ROS_DEBUG("SBPL: allow_unknown: %u", allow_unknown_);
 
     name_ = name;
     costmap_ros_ = costmap_ros;

When I run the code, it prints "SBPL: allow_unknown: 0", thereby proving that it actually runs your code and correctly reads the parameter.

Did you attempt my instructions above? Are your results different? I.e., does the allow_unknown parameter affect whether SBPL plans into unknown space or not?

[MartinPeris]

Thank you for taking the time to debug this. I am attempting your instructions above on a clean install of melodic right now (my development environment was actually kinetic and it works there). Will report back ASAP

[mintar]

FYI, I've tried this on Noetic, but there shouldn't be a big difference between melodic and noetic. Thanks for following up on this!

[MartinPeris]

I know what happened. I happily made this modifications but the planner I was testing the allow_unknown parameter on was actually navfn!!

I am closing this PR in sheer shame and will open a new one when I have something that actually works.

I profusely apologize for wasting your time. m(._.)m

[mintar]

LOL, no problem! Looking forward to the next PR!

[MartinPeris]

I am back to restore my honor! The PR code works, but the navigation stack configuration of move_base_sbpl_fake_localization_2.5cm.launch sets up costmap_2d in a way that makes the unknown space be considered free space long before the costmap reaches the sbpl planner.

In particular, the global costmap static layer uses track_unknown_space: false by default and the obstacle layer (VoxelLayer) is marking unknown space as free space, so I disabled it for testing.

Here is what you can do to make it work:

diff --git a/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml b/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml
index 10a4b90..df3e941 100644
--- a/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml
+++ b/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml
@@ -6,3 +6,7 @@ global_costmap:
   publish_frequency: 0.0
   static_map: true
   rolling_window: false
+  static_layer:
+    track_unknown_space: true
+  obstacle_layer:
+    enabled: false
diff --git a/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml b/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
index 9f9b3dc..2cc4aae 100644
--- a/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
+++ b/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml
@@ -4,3 +4,4 @@ SBPLLatticePlanner:
   allocated_time: 5.0
   initial_epsilon: 3.0
   forward_search: false
+  allow_unknown: false

If you send a goal into an unknown area you will get an output similar to this:

[ WARN] [1656562556.671413503, 61.400000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3000 seconds
[ INFO] [1656562556.671673743, 61.400000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562557.034689471, 61.800000000]: Solution not found

[ WARN] [1656562557.051988724, 61.800000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3000 seconds
[ INFO] [1656562557.052279897, 61.800000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562557.407310553, 62.100000000]: Solution not found

[ INFO] [1656562557.421677625, 62.100000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562557.756666124, 62.400000000]: Solution not found

[ WARN] [1656562557.790346019, 62.500000000]: Rotate recovery behavior started.
[ INFO] [1656562563.910470609, 68.700000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562564.267239400, 69.000000000]: Solution not found

[ INFO] [1656562564.292977354, 69.000000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562566.453954001, 71.200000000]: Solution not found

[ WARN] [1656562566.471134955, 71.200000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 2.1000 seconds
[ INFO] [1656562566.471400641, 71.200000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562568.609459825, 73.300000000]: Solution not found

[ WARN] [1656562568.634681636, 73.300000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 1.9000 seconds
[ INFO] [1656562568.635049319, 73.300000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562569.021682505, 73.700000000]: Solution not found

[ INFO] [1656562569.022313925, 73.700000000]: [sbpl_lattice_planner] getting start point (15.629,27.95) goal point (30.2293,25.4275)
[ INFO] [1656562569.365483987, 74.000000000]: Solution not found

[ERROR] [1656562569.390470363, 74.200000000]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

The robot will rotate in place a couple of times, but that is because those are the recovery behaviors activated when a plan can not be found. In the end it will abort because a valid plan could not be found.

Users interested in the allow_unknown feature should make sure that the costmap that is sent to the SBPL planner actually contains "unknown space" and that it has not been translated into "free space" by an upper layer.

[mintar]

I totally forgot about this one. Sorry for the delay and thanks for your contribution!

[MartinPeris]

Awesome! Thanks for taking the time to review this.