Panav/feat/osm graph loader#6219
Conversation
There was a problem hiding this comment.
Testing OsmGraphFileLoader (nav2_route)
Build
colcon build --packages-select nav2_route --symlink-install
source install/setup.bashOffline tests
./build/nav2_route/test/test_osm_graph_file_loader
# expect: [ PASSED ] 23 tests.
colcon test --packages-select nav2_route --ctest-args -R "cpplint|uncrustify|xmllint"
# expect: 0 failuresEnd-to-end
Datum: lat 40.711, lon -74.007
1. navsat_transform
ros2 run robot_localization navsat_transform_node --ros-args \
-p use_local_cartesian:=true -p wait_for_datum:=true -p datum:="[40.711,-74.007,0.0]"2. Feed odom + GPS, set datum
ros2 topic pub -r5 /odometry/filtered nav_msgs/msg/Odometry \
"{header: {frame_id: odom}, pose: {pose: {orientation: {w: 1.0}}}}" &
ros2 topic pub -r5 /gps/fix sensor_msgs/msg/NavSatFix \
"{status: {status: 0}, latitude: 40.711, longitude: -74.007}" &
ros2 service call /datum robot_localization/srv/SetDatum \
"{geo_pose: {position: {latitude: 40.711, longitude: -74.007}, orientation: {w: 1.0}}}"3. Route server
GRAPH=$(ros2 pkg prefix nav2_route)/share/nav2_route/graphs/sample_graph.osm
cat > /tmp/osm_params.yaml <<EOF
route_server:
ros__parameters:
graph_filepath: "$GRAPH"
graph_file_loader: "OsmGraphFileLoader"
OsmGraphFileLoader: {plugin: "nav2_route::OsmGraphFileLoader"}
route_frame: "map"
edge_cost_functions: ["DistanceScorer"]
DistanceScorer: {plugin: "nav2_route::DistanceScorer"}
operations: ["AdjustSpeedLimit"] # non-empty required
AdjustSpeedLimit: {plugin: "nav2_route::AdjustSpeedLimit"}
EOF
ros2 run nav2_route route_server --ros-args --params-file /tmp/osm_params.yaml4. Activate
ros2 lifecycle set /route_server configure
ros2 lifecycle set /route_server activate
# expect log: Loaded OSM graph: 5 vertices from 4 sectionsVisualize
ros2 topic echo --once /route_graph # 13 markersRViz2: MarkerArray on /route_graph, Fixed Frame map, Durability Transient Local.
|
@SteveMacenski Can you take a look :D |
|
Please fix the compilation, precommit, and DCO issues before I take a detailed look :-) |
|
|
||
| | Parameter | Type | Default | Description | | ||
| | --------- | ---- | ------- | ----------- | | ||
| | `highway_filter` | string[] | `[]` (keep all `highway=*` ways) | Allowlist of OSM `highway` values to keep, e.g. `["track", "path", "service", "unclassified", "residential"]` | |
There was a problem hiding this comment.
I jumped the gun here given what was being discussed on the issue such as filtering routes.
In hindsight, I should revert this change.
There was a problem hiding this comment.
removed this filtering every tag is retained now, the type of 'way' is also retained to be used later.
|
|
||
| A small example is provided in `graphs/sample_graph.osm`. | ||
|
|
||
| Note: OSM node ids are 64-bit and cannot be the Route Server's 32-bit node id, so unlike the GeoJSON |
There was a problem hiding this comment.
This seems like a problem. The solution is either to truncate the OSM IDs or to increase the node id to 64 bits.
There was a problem hiding this comment.
Would it be a nice idea to encode it, something like
std::string osm_id_str = std::to_string(osm_id); graph[idx].metadata.setValue("osm_id", osm_id_str);
Changing nav2_msgs woudn't be desirable, truncation would be a one way process and could still cause corruption.
There was a problem hiding this comment.
I think we should increase node_ids to 64 bits
| Coordinates are converted from WGS84 latitude/longitude into the map frame using | ||
| robot_localization's `FromLLArray` service, so the graph shares a single datum with the robot's | ||
| localization rather than introducing a second one. **This requires `navsat_transform_node` to be | ||
| running when the graph is loaded** — the graph is loaded during the Route Server's lifecycle |
There was a problem hiding this comment.
The - tells me this is AI generated. Can you please write documentation in your own words so its more clear to users?
The loader resolves this implicit topology by treating shared nodes (and way endpoints) as junctions, splitting each way at its junctions, and emitting one directed edge per inter-junction section. The intermediate shape nodes are not turned into graph vertices, keeping the graph sparse
This is particularly obtuse. Is this is a standard way to do this in OSM? Seems like defining connected nodes shouldn't be that hard and seems like you may have invented some way of shoehorning the data format into what we need (which makes me think perhaps there's a better way or I'm misunderstanding)
Signed-off-by: Pana1v <praajarpit@gmail.com>
…e conversion Signed-off-by: Pana1v <praajarpit@gmail.com>
- export robot_localization (the installed header exposes the FromLLArray service type) and move the geographic_msgs include into the .cpp (IWYU) - parse OSM node/nd attributes with Query*Attribute, skipping malformed elements instead of silently storing id/coords 0 - warn (and document) when a self-loop spur section is dropped - make the FromLLArray service name and timeout parameters, and check the wait_for_service result before invoking - README: note OSM ids are not preserved and that loading blocks on navsat_transform during the lifecycle configure transition - tests: oneway value table, highway-filter allowlist, <2-ref ways, missing coordinates, self-loop edge skip, edge destinations + ids, parallel edges Signed-off-by: Pana1v <praajarpit@gmail.com>
Signed-off-by: Pana1v <praajarpit@gmail.com>
85bb3f3 to
cc0c381
Compare
|
While I am adding a sample osm which is a simplified implementation, should we also include documentation for someone to import an OSM map and edit it using a tool such as JOSM ? @SteveMacenski What are your thoughts.. |
That would be a good follow up PR! First, I think we should focus on getting this to build so we can review and chat more 😉 |
A .osm fed to nav2_route is a purpose-built route graph, so the loader should load every way faithfully rather than second-guess which ways are "real" via highway=*. Which edges are preferable is a cost decision that belongs to the edge scoring plugins, which can read each way's tags. - Drop the highway_filter parameter and the implicit highway=* requirement - Delete shouldKeepWay(); every parsed way is now kept - Remove the now-dead unordered_set include and member, and the two filter unit tests; add a test asserting an untagged way is kept - Clarify README topology and keep-all-ways prose; drop highway_filter row Signed-off-by: Pana1v <praajarpit@gmail.com>
2 participants
Basic Info
Description of contribution in a few bullet points
Added OSM map loader along the lines of GeoJSON loading, added appropriate unit test for the same.
Description of documentation updates required from your changes
osm_graph_file_loader.highway_filter,from_ll_service,from_ll_service_timeout) should be added to the configuration page ondocs.nav2.org. (The in-package
README.mdis updated in this PR.)OsmGraphFileLoadershould be added to the plugins page on docs.nav2.org.navsat_transform_node(the
FromLLArrayservice) to be running, since the load blocks on it during lifecycle configure.Description of how this change was tested
test/test_osm_graph_file_loader.cppcolcon testpasses, includingcpplint,uncrustify, andxmllint.Future work that may be required in bullet points
maxspeed, surface, access),enabling the existing
SemanticScorer/AdjustSpeedLimitplugins — the planned follow-on PR.straight-line scoring between junctions.
.pbfsupport;<relation>-based turn restrictions;oneway:conditionaland access tags.OSM
int64ids don't fit the 32-bitnodeid)For Maintainers:
backport-*.