Repository navigation
easynav_bigmap_maps_manager: a large NavMap, fine around the robot and coarse far from it - #183
Open
jmguerreroh wants to merge 9 commits into
Open
jmguerreroh wants to merge 9 commits into
jmguerreroh wants to merge 9 commits into
Conversation
…d coarse far from it The NavMap is cut into tiles and simplified into a quadtree of levels of detail once; every cycle the coarsest tile each place allows is put together around the robot (stitched, no cracks), the borders of the map and the sensed obstacles near it refined as far as the inflation reaches, and the sensed obstacles forgotten beyond forget_distance. Configuration: the map, cell_size min / max, min_radius, max_radius, forget_distance and the filters. Needs navmap_core's keep_vertices, cut_along_lines and refine (NavMap PR).
- Tiles kept without the NavMap accelerations (BVH, adjacency, locate grids), the map cut in place instead of copied, each tile taken from it just before it is simplified, and the memory the building threads leave free given back (malloc_trim): 40 to 70 % less resident memory. - rebuild_period (default 0.5 s), and the README of the advanced parameters.
…together Stitching inserted into a coarse edge on a grid line every vertex of the other side's tiles within 0.5 m plus half the coarsest cell in height (8.5 m with cells up to 16 m), so on maps with floors over each other a NavCel of one floor could be split to reach a vertex of another (in the parking garage, the roof became reachable from the floor below). Now a vertex goes, on each side of the line, into the covering edge nearest in height, and into an edge of another chain of the line only if it is within 0.5 m of it in height. Maps with one floor give the same map as before; test FloorsOverEachOtherStaySeparate.
…down; no layer updates before the map Sensed points were placed at the height of the ground under the robot plus their height in the footprint frame, which is flat in TF (2D odometry) while the robot stands tilted on a ramp: going down, the ramp ahead looked higher than the map there, was taken for an obstacle and blocked the ramp (in the parking garage the robot went back and forth between the two ramps down). The points are now turned by the tilt of the NavCel under the robot; test GoingDownARampTheRampAheadIsNotAnObstacle. Layer updates (map_updates) are not published while a rebuilt map waits to be published: they were of a geometry the subscribers did not have yet (RViz: "belongs to another geometry").
…away The sensed obstacles were remembered until the robot was forget_distance from them, so one that moved (a person crossing a corridor) left a trail of obstacles that could close passages around the robot. They are now an occupancy memory in log-odds, as the NavMap maps manager's obstacle filter: a point on a cell is a hit, a ray from the robot through it a miss (memory.clearing, memory.p_hit, memory.p_miss), and memory.decay_time forgets what is not seen again. The rays cross a cell if they pass within half a metre of its height. Tests: AnObstacleThatMovesAwayIsCleared, WithoutClearingAnObstacleIsRemembered, AnObstacleNotSeenAgainDecays.
…n at; the cost of a cycle The perceptions were taken to the footprint frame and placed on the map with the robot's pose of now: a cloud taken while the robot turned in place landed rotated, walls seen a fraction of a second earlier fell on free ground as obstacles, and those behind racks, out of sight once the robot stopped, were never cleared (in the large warehouse the robot stayed boxed in after its first goal). They are now taken to the map frame with TF at their stamps; the tilt correction on ramps turns each point about the robot as before. For measurements: every cycle the NavState gets "map.bigmap_stats" (rebuilds so far, the last rebuild's select, assemble and detail times and NavCels, this cycle's sensing and filter times), also published on <node>/<plugin>/stats while someone listens.
With the perceptions taken to the map frame by TF, a localizer that gives the robot's pose in 3D (its pitch on a ramp) already tilts them: the correction for a flat TF was applied twice and the ramps were taken for obstacles again (the parking garage with a 3D ground truth). The points are now turned from the robot's up in TF to the normal of the ground under it.
BigMap and the NavMap obstacle filter placed every cloud on the map with the robot's latest pose. A cloud may be up to the sensors' forget_time (0.5 s) old and is used in every cycle until the next one arrives, so while the robot turned in place its points landed rotated: walls 14 m away fell across free aisles as lethal obstacles, and once the robot left, out of the sensor's range, they were never cleared (in the large warehouse a robot on the fixed NavMap stayed boxed in). Both now fuse each perception with the TF at its stamp. Turning in place at 0.8 rad/s for 40 s in the warehouse, where the map has every obstacle: BigMap remembered 594 made-up obstacles before and 0 after; the NavMap filter peaked at 27 cells before and 2 after.
This branch has not been deployed
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Replaces #180 with a different design. That PR's implementation (RTIN meshes of a DEM, about
15 000 lines) is not continued here: this one builds on NavMap's own tools and is about 2 000
lines.
Depends on EasyNavigation/NavMap#47 (
keep_vertices,cut_along_lines,refine).The rolling CI builds NavMap from
rolling, so it will fail on this PR until that one is merged.What
easynav_bigmap_maps_manageris a maps manager for NavMaps too big to plan on at full detail(a warehouse, a town). The map is fine around the robot and coarse far from it. The obstacles
the sensors see are added near the robot and forgotten when it is far from them.
How
Once, at start. The NavMap is cut into tiles (
cut_along_lines) and simplified into aquadtree of levels of detail (
simplify, with the corners and border junctions of each tile keptvia
keep_vertices).simplifykeeps the topology: no hole closes, no wall opens, howevercoarse the level. This takes 8 s for the 2.5 M NavCels of the warehouse and 42 s for the 14 M of
2 x 2 km of Madrid (cells of 0.5 to 16 m), using every core only during this step.
Every cycle, when the robot has moved a few metres or the sensed obstacles changed (at most
twice a second):
min_radius, the borders of the map and the sensed obstacles are refined (refine) tocell_size.min, out to the inflation radius.Assembling takes 7 ms for the warehouse and 45–65 ms for the town; adding the detail takes
30–60 ms and 70–190 ms.
Sensed obstacles are the perception points between
obstacle_min_height(which grows withdistance) and
obstacle_max_heightabove the map's surface. Each cloud is placed on the map withthe TF at its stamp (with the latest pose, a cloud taken while the robot turns lands rotated and
makes up obstacles that, once out of range, are never cleared), and the heights are measured along
the surface's normal, correcting only the tilt the robot's TF lacks, so the ramp ahead is ground
going down a ramp. Points next to a hole of the map are
the map's own obstacles and are ignored. They are kept as an occupancy memory in log-odds, as the
NavMap maps manager's obstacle filter: a point on a cell is a hit, a ray from the robot through it
a miss (
memory.clearing,memory.p_hit,memory.p_miss, optionalmemory.decay_time), soan obstacle that moves away stops blocking; they are forgotten beyond
forget_distance.Results
2 x 2 km around the Puerta del Sol, Madrid (ground from the IGN, buildings from the Catastro;
13.8 M NavCels at 0.5 m). 30 routes of 230 to 1481 m, with
easynav_navmap_planner's A*, the samesafety band on every map, and the robot replanning every 30 m (the
city_routescampaign ofbigmap_experiments):
Paths within 1 % of those on the full map, planned 69 times faster; the map and the planner's
graph are made again in 263 ms every 30 m.
In Gazebo (Summit XL with its 3D lidar, RPP, A*, the true pose instead of a localization), against
the NavMap maps manager on the same map with its obstacle filter, three runs of each:
large_warehouse_world(700 x 450 m, 2.5 M NavCels)No stall and no contact (closest 0.94 m from the robot's centre). A rebuild there takes 10–21 ms to
assemble, 20–37 ms to add the detail and 9–18 ms for the inflation (about 31 k NavCels). Floors one
over another are not stitched together: in a three-floor parking garage the robot went up and down
the ramps and reached every goal on its floor; in two of three runs, though, sensed obstacles at
the top of the down ramp closed it for a while and the robot detoured (28–65 m). Not diagnosed yet.
Also in this PR: the NavMap maps manager's obstacle filter had the same problem with the
latest pose (turning in place for 40 s at 0.8 rad/s in the warehouse made up to 27 lethal cells), and
now also fuses each cloud with the TF at its stamp (
fuse(..., true)).Tests
test_tiled_lod: the levels, no cracks between tiles of different levels, holes and narrowpassages kept far away, floors one over another kept apart, and layers kept.
test_bigmap_plugin: the plugin as EasyNav runs it:decay_timeone not seen again fades;colcon test --packages-select easynav_bigmap_maps_manageron this branch, with the NavMap PR:55 tests (with the linters), no failures;
easynav_navmap_maps_manager, 144 tests, no failures.