Make local voxel map z-axis center aligned with robot odometry z

[XuRobotics]

Currently, the global and local map z-axis ranges are assumed to be the same (or very similar). Therefore, the bottom (i.e. z-axis origin) of the local voxel map is just set the same as that of the global map. However, when planning in 3D, we sometimes do want different z-ranges for local and global voxel maps.

We need to change the local voxel map center to be aligned with the robot odometry instead. Be careful that we may also need to adjust the storage map to adapt to this change. The code changes should be minimal, but extensive testing is needed to make sure edge cases are handled properly by running 3D planning in various cluttered environments.

Update: Would be great if we can (1) reduce the dimensions of the storage map, or (2) get rid of the storage map completely.

For (1) one idea is to have the storage map's dimensions be resonably (e.g. 2 times) larger than the local map, and reinitialize it once in a while to be centered around the robot.

For (2) we need to think about how to initilize and update the local map efficiently (running up to 10hz).

Either case, we will need to think carefully about how to reinitialize maps (i.e. you need to accumulate point clouds, instead of throwing the history point cloud information away every time when you move the local map). This paper can be helpful: https://arxiv.org/abs/1703.01416.

[nathanmbaker10]

Hi Xu, Nice to meet you! I'm Nathan. Undergrad in CMPE and Accelerated Master's Candidate for ROBO program. Super interested in working on quadrotor autonomy. Thought I'd give this issue a try.

[XuRobotics]

Hi Nathan, Glad to hear your interest! Sounds good, feel free to reach out if any clarification regarding this issue is needed!

[nathanmbaker10]

My plan is to write some code to modify the ..../mapper/src/local_global_mapper.cpp file.

Current idea is to set local_map_info_.origin.z to lidar_position_odom(2) in the LocalGlobalMapperNode::processCloud() function, somewhere after line 287.

I understand that the processCloud function which is part of the cloudCallBack gets called whenever ros messages are posted to the cloud_name_ because the NodeHandle object ('nh') subscribes to this topic on line 7 as part of initialization. The other things that are going on with regard to the lidar hardware are beyond me at the moment, but I understand that all these files are communicating via ROS messages generally according to the higher level diagram on the readme.

[nathanmbaker10]

Questions:

1. What is the importance of the timer (line 314)? Current idea is to set the z origin for the local map before timer starts, but would love to understand more about this timer. Is this a global timer for ros?

2. The robot odometry is constantly changing. Is odometry data really being sent and retrieved to and from a non-local cloud? This seems very inneficient. In other words, is this cloud local to the drone, or is the cloud located somewhere else?

3. What exactly is the storage map? Where is it being stored? Would love to understand further the relationship between the storage map, global map, and local map. I assume from the comments that I've read that storage_map z origin and local_map z origin need to be the same? If so, maybe I also need to modify storage_map in the process cloud function?

Should be ready for a pull request by tomorrow if I can get these ideas clarified.

Best, Nathan

[XuRobotics]

What is the importance of the timer (line 314)? Current idea is to set the z origin for the local map before timer starts, but would love to understand more about this timer. Is this a global timer for ros? -- Don't worry about timer, that's used for timing how much time each function takes, you can ignore it for now.

The robot odometry is constantly changing. Is odometry data really being sent and retrieved to and from a non-local cloud? This seems very inneficient. In other words, is this cloud local to the drone, or is the cloud located somewhere else? -- Yes you're right. The local map should be centered around the robot, which means it is constantly changing. So the only thing that needs to be changed can be found in line 198 - line 203 of local_global_mapper.cpp. You can see that the local map has the same center as the storage map along a direction. Probably just removing that line 200 that sets the local_origin_odom will be sufficient.

What exactly is the storage map? Where is it being stored? Would love to understand further the relationship between the storage map, global map, and local map. I assume from the comments that I've read that storage_map z origin and local_map z origin need to be the same? If so, maybe I also need to modify storage_map in the process cloud function? -- Storage map only exists to speed up the generation of the local map. A storage map is a map that is large and has the same resolution as the local map. Given a local map center and dimension, we just crop a submap from the storage map. Global map has similar dimensions as storage map but is much more coarsely discretized. The idea is that the global planner plans in global map and the local planner plans in the small local map, thus increasing computation efficiency, while not comprising much on the optimality (local planner can still deviate from the global path, and fly through small gaps if they exist). For more info you can read this paper.

[XuRobotics]

As stated in the issue description: " the code changes should be minimal, but extensive testing is needed to make sure edge cases are handled properly by running 3D planning in various cluttered environments."

Also it would be great if you can think of ways to either (1) reduce the dimensions of storage map, or (2) get rid of it completely. Please see the updated description of the issue.

[nathanmbaker10]

Ok thanks for the clarification. I'll take a closer look at the paper. I agree that removing line 200 would probably work. This ensures that local_voxel_map is set correctly. Is this all that matters? What about local_map_info which exists outside the scope of cropping the localMap?

I'll take a look at new description. Do you want me to test this using a simulator?

[XuRobotics]

Is this all that matters? What about local_map_info which exists outside the scope of cropping the localMap? -- Currently, the voxels should be automatically set as occupied if they lie in the part of the local map that is outside the storage map. You are right, there are some edge cases that you will need to handle, and it would be great to test in the simulator with different environments. You can change environment by changing world_model parameter in autonomy_sim/gazebo_sim/gazebo_utils/launch/full_sim.launch. See mrsl_quadrotor for all environments available. I can create new environments for you to test too, if you find it necessary.

[apirjani]

Hi Xu, I'm currently trying to contribute to this issue and would appreciate clarification on the following:

1. In an earlier comment, you said that the storage map exists to speed up the generation of the local map. I understand that the local map is generated by cropping a submap from the storage map (which is a larger map with the same resolution), but how does using a storage map increase the speed at which the local map is generated? Also, is the storage map only used when initializing the local map, or is it also used when updating it? I'm hoping that these clarifications will help me in thinking about how I should implement the dimension reduction or elimination of the storage map.

2. In regards to testing the 3D planning in various cluttered environments, what types of edge cases may need to be handled after making the local voxel map z-origin aligned with the robot odometry? Hopefully with this clarification, I'll have a better idea of the types of tests I should run, and what I should be looking for in the tests.

3. For testing, should I be using Gazebo or Unity? I've tried setting up the Unity Sim, but it seems like I don't have access to the simulation pipeline.

[XuRobotics]

Hi Andre,

Good questions!

1. "...but how does using a storage map increases the speed at which the local map is generated. Also, is the storage map only used when initializing the local map, or is it also used when updating it?" -- Yes, the raycasting and map updating all happen in the storage map (but we limit the raycast range to bound the computation complexity). The local map can be cropped instantly from the storage map (just creating a new small voxel map, and indexing the values in the big storage map), and that is why it is efficient. Suppose that you do not do the update in the storage map, then you have to re-do the raycasting from scratch whenever you move and create a new local map, this will cause delay in getting the map, and also safety issues (history point clouds are discarded).

2. In regards to testing the 3D planning in various cluttered environments, what types of edge cases may need to be handled after making the local voxel map z-origin aligned with the robot odometry? -- Image this: the robot has positive z velocity (flying upwards), close to the map boundary, and the stopping policy is triggered, then the robot might overshoot to outside the map z boundary, since the stopping policy is "blind" (just try to stop ASAP but along a straight line). Then the robot will never be able to replan again since the start is outside the map.

3. For testing, should I be using Gazebo or Unity?
   -- Gazebo is the way to go. You can try different environments by changing it in the full_sim.launch.

[apirjani]

Hi Xu,

I've been running some tests on the sim (with and without the code modifications to make the local map z-axis origin aligned with odom), and the good news is that after removing line 200 in .../mapper/src/local_global_mapper.cpp, the local map box in RViz is now clearly moving in the z direction with the robot. However, I have a few questions:

1. It seems that while trying to run 3D planning, I cannot get the robot to move in the z direction (except for during take-off). For example, I've tried setting a waypoint that has z = 6.00 when the drone's starting position has z = 1.05, and the drone only travels to the x-y position of the waypoint, ignoring the z component. Why do you think this may be?

2. In your example edge case above, I'm not entirely sure I understand the exact scenario you're imagining. If the robot has a velocity in the positive z direction, and is close to the map boundary, wouldn't the robot just stop as soon as the stopping policy is triggered? Otherwise, what is the point of the stopping policy? Maybe I need to gain a clearer understanding of the stopping policy?

Thanks in advance, Andre