Recreating Maze Experiment from Paper

[danielduberg]

Hi, First of all, thank you for this nice project and paper!

For the last week I have been trying to recreate your Maze experiment from your paper. However, I never manage to make the MAV explore the whole environment and after a while, it deletes the whole tree. I read in another issue (#22) that you used the reconstruction_planner.yaml file for the Maze experiment as well, the only difference being that you changed VoxelWeightEvaluator to NaiveEvaluator, so I did that.

Below you can see the config file I used config.yaml

I removed all the things related to Unreal from the launch file since I am using only Gazebo with the PX4 flight stack. Here you can see my launch file test.launch

This is the bounding volume config I use bounding.yaml the only difference from your Maze.yaml being that z is from 0 to 3, instead of -1.5 to 1.5 since the origin of the MAV is on the floor in my case.

I also recorded a video so you can see what is happening. The video is speed-up 2X. https://youtu.be/n31gT60CPY4 In the video, you see the Voxblox map, the path the MAV has moved (red thick line segments), the planned trajectory (the green thick line segments), the tree (thin red/green line segments), the gains (the red/green spheres), the proposed trajectory (the TF), the red MAV following the proposed trajectory using my controller.

Do you have any ideas what I can do to make it explore the whole environment and so that it maintains the same tree, as in your experiments?

Any help is very appreciated, thank you!

[Schmluk]

Hi @danielduberg

Thank you for your interest. Judging from your video it appears that the gain computation does not work as intended (since the majority of them seem to be 0). Could you enable all visualizations of the gain (including the text + views), and see which voxels are counted?

One common source of errors here is that the target bounding volume is expected in odometry frame, i.e. with respect to the drone's start position. If the drone does not start at [0,0,0], the bounding box needs to be adjusted for it.

Regarding parts of the tree dying, this can happen when the map changes in such a way that segments are no longer traversable. In particular if the robot moves close to a wall it can be that it's connection to the tree becomes infeasible, essentially cutting off the whole tree. However, this should usually not be a problem since it's quickly rebuilt.

Best wishes, Lukas

[danielduberg]

Thank you @Schmluk, I will look into the target bounding volume. Is it using the drone's pose when you call the /planner/planner_node/toggle_running as the origin? And is the bounding volume specified relative to this pose?

So, if I move the drone to [0, 0, 1.5] before calling /planner/planner_node/toggle_running should the z value of the bounding volume be [-1.5, 1.5] instead of [0, 3]? Even though the drone knows it started from z 1.5?

I was also thinking it has something to do with the gain computation. I will enable all visualizations of the gain and record a new video. I have looked at it before but I was not sure what it should look like.

Thank you very much for your help!

[danielduberg]

Here is a video where all visualizations of the gain (including the text + views) are enabled. https://youtu.be/QFRZvOeyqBY

Here is the same video at 2x speed https://youtu.be/T6LpHpWSj60

Here is the same video at 2x speed and higher resolution and zoomed in so it is possible to read the text https://youtu.be/1APSSIDET7g

Sorry that it is a bit difficult to read the text in the video.

I use the same configuration as before. I started the exploration process at [0, 0, 0], meaning the drone spawned at [0, 0, 0] and I called the /planner/planner_node/toggle_running service when the drone was at [0, 0, 0].

Again, the drone gets stuck and just moves back and forth. Can you spot anything that is going wrong?

If you want I can upload the rosbag from this run containing the topics:

• /planner/trajectory_visualization
• /planner/voxblox_node/mesh
• /tf
• /tf_static

[Schmluk]

There still seems to be something going wrong with the gain computations. E.g. at the beginning (and at other times) many gains are zero which they definitively shouldn't be but at some points it looks like it's doing what it's supposed to.

To make sure the bounding volume is not the culprit here you can try to set everything to +/- a large value except for z-max?

As a second verifiation, are you using voxblox as map representation? If so you could try to visualize the voxblox server that is inside the planner (it only subscribes to map updates from the /planner/voxblox_node and uses it for lookups) to check that the map available to the planner makes sense. I can try to double check that the rest works as intended.

[danielduberg]

Yes, I am using Voxblox as map representation. I will try to visualize the voxblox server inside the planner. I will also increase the size of the bounding volume as you proposed.

Thank you, I will come back when I have new results.

[danielduberg]

Unfortunately, increasing the bounding volume does not seem to help.

Here is a video visualizing the different planner_node topics at the beginning. https://youtu.be/TV821S9zktY Does this look okay to you?

and here is a video when the drone explores https://youtu.be/EdMQ6fn_WMQ

I used a trajectory_generator clearing_radius of 1.0 in this case.

How sensitive is the planner of getting close to obstacles? Can one of the problems be that my controller overshoots a bit sometimes, resulting in the drone thinking it is inside the wall and therefore discards most of the tree?

[Schmluk]

Hi @danielduberg,

I double checked some potential issues, could you checkout #37 and check whether it solves your problem? It worked fine on my machine. I also added an example config for the exploration planner (which should be extremely similar to yours).

Some other considerations I noticed regarding performance / strange behavior in your setup:

• The planner uses the ESDF map for lookups, if you use the 'clear_spehere_for_planning' in voxblox, the esdf around the starting position will be filled in with large values. This explains why there were no gains where the planner started, even though there is no mesh there. I would recommend not using this voxblox param but set the 'clearing_radius' in the planner config to e.g. 1.0.
• There is a bit of latency between sensor data being integrated, published to the second voxblox server and the gains being updated in the planner. Since you move very quickly between viewpoints this latency might induce strange behavior.
• Similarly, the number of new samples is dependent on the used hardware and time available. If the drone moves very fast it can occur that too few local samples are added to continue exploration. If these two points are an issue for your case you can try setting the replan_pos/yaw_threshold of the planner lower or enforce a minimum number of samples by setting min_new_tries/segments.

Let me know if this helps.

[danielduberg]

Big thanks @Schmluk!

I will look into these things and try the things you proposed first thing tomorrow morning.

Also, thank you for the detailed explanation. My previous runs have been on an Intel Core i7 8700K with 64 GB of RAM and an AMD Ryzen 5 3600x with 32 GB of RAM, so I think/hope hardware wise it should be fine.