TP-Space navigation: include info for closest obstacles not only collisions

[jlblancoc]

Try to include that info into precomputed tables: it may take more memory, but may allow robots to navigate more correctly finding the middle of gaps (safer navigation).

[jlblancoc]

More details on what is to be done here:

• [x] Update mrpt::nav::build_PTG_collision_grids() and associated data structures to add a second LUT (look-up-table), not for collisions, but for the minimum clearance between any part of the vehicle (which may be of any shape) and any "obstacle-cell" in a grid (x,y), for any alpha (TP-Space path parameter) and for a small set of distancies along each trajectory. Instead of a 4-dimensional LUT, it may be more efficient to reuse MRPT 2D grids (just like the current collision LUT) storing, at each cell, a CMatrix with rows and cols being the alpha and distance index.

• [x] Update (de)serialization so the new LUT is also loaded/saved to disk and needs to be computed only once.
• [x] Create a new method (in mrpt::nav::CParameterizedTrajectoryGenerator?) to evaluate the minimum clearance of a path given its alpha and its distance along the path. Clearances may have to be interpolated somehow between the small discrete set of distances for which clearance was precomputed.
• [ ] Update the code in reactive navigators so data from the new LUT is merged for each obstacle point at each instant. See CReactiveNavigationSystem::STEP3_WSpaceToTPSpace()
• [ ] Make the clearance accesible as a new parameter to the reactive navigation engine, and decide good default weights so the robot tends to avoid paths too close to obstacles, even if theoretically collision-free.

[chatrasen]

@jlblancoc In the previous LUT we are storing the (a,d) pair which make the robot land in that cell starting from the position (0,0) right?

And in the new LUT that we want to design, we will have to simulate trajectories from the cell we are storing information in and then find the minimum clearance? Is that what we have to do? If wrong, please correct me.

[jlblancoc]

The previous LUT is built in mrpt::nav::build_PTG_collision_grids(). In this code it can be seen how an entry is added to the LUT only when the shape of the robot collides with the cell... which may be quite different that the robot origin (the local 0,0 coordinats on the vehicle) reaches that cell ;-)

The new code might be incorporated inside build_PTG_collision_grids() as well, probably reusing all the outer loops. The missing part is evaluating the minimum distance from (obs_x,obs_y) to the robot polygon. May need to use geometry functions in mrpt::math.

[chatrasen]

@jlblancoc Thanks, got my first doubt cleared.

For the new LUT, what you mean to say is, inside the same if as you mentioned in the above code, store the minimum distance from (obs_x, obs_y) corresponding to the (a,d) pair and keep rest of the code as it is, right?

[dheerajgattupalli]

@chatrasen I dont think we can write it inside the if condition...

For each obstacle point we have to compute the distance from all points on the robot polygon to obstacle point and save the minimum a,d.. so this part should be written outside the if condition.. @jlblancoc correct me if i am wrong anywhere

[jlblancoc]

@dheerajgattupalli is right, the new code should go outside of the if However, note that the closest distance may lie in the middle of a segment, so checking for vertices should not be enough. Hint: TSegment2D::distance()

Also, remember that to avoid excessive memory usage, clearance should not be evaluated for all d values (n for loop)...

[chatrasen]

@dheerajgattupalli why are the npoints obstacle points? They have just been simulated from the trajectory.

[dheerajgattupalli]

@chatrasen true npoints are points from trajectory not obstacle points ... i edited my comment :-)

[jlblancoc]

Thank you @chatrasen for your great contribution in Pull request #202 !! I just marked as done the first 3 checkboxes above. However, there are some small issues that should improved as added in code comments in the PR. It would be great if you or someone else could go on with this line, otherwise it will be addressed in the future, no worries...

[chatrasen]

@jlblancoc The CReactiveNavigationSystem::STEP3_WSpaceToTPSpace() function in this link takes in one parameter as the vector of minimum distance of the obstacle for various alphas. To incorporate the clearance grid, should I use a pair inside the vector or create a new vector for this purpose. Because using a pair will change the code a lot for the existing users I guess though it is convenient for a new user

[jlblancoc]

Good point @chatrasen ! Indeed, that part must be changed... I guess that it should be worth changing it to make it "right" and more maintainable in the future: one new struct could be created (instead of a pair, because we have a pair for now, but who knows in the future?) and use a vector of that struct.

[chatrasen]

Yea right, this would be take care of any possible changes in future. Will be working on this soon. Presently working on the GSoC apllication :P

[jlblancoc]

Sure! First is first ;-)