I’m now getting to some detailed integration testing and the node achievement logic is absolutely beautiful. I’m testing in some odd situations and it triggers exactly where someone would say along the actual trajectory traveled where it should using the dot product of the edge vector bisector with the distance vector.
B.E.A.U.T.I.F.U.L.
There’s still much integration testing to do of all the features and adding in the appropriate BT nodes to integrate into a navigation application, but I hope to be ready for beta testers in a couple of weeks’ time.
Anyone interested in giving it a whirl?
Edit: Figure showing is better than telling, see this YouTube video where I showcase one of my current tests
You can see in it that the server calculates the route and passes it to the navigation stack to process it. As the robot passes nodes, it triggers a feedback message to the test client indicating changes in state of tracking from one Edge to the next with its associated Node IDs. Not shown is the backend Route Operations which can be triggered on entering / exiting a particular edge or on achieving a particular node (like changing speed or opening a door).
This showcases the MVP level capabilities, which I’m still testing all the different edge situations to make sure things are rock solid at an application level - though given the density of unit testing I’ve already written, I’m pretty confident that it works except for little hiccups. But this is one big and important step, this is the first time all of the software written has been executed in concert and I’m really happy that things seem to be working as I would expect!
This demo will be made available in the PR to come as a nav2_simple_commander route server demo.
hi can you upload your source code to test Route Server ? I try to open source Nav2 Route Server running with turttlebot3 but it’s not working now, i have poblem with my params of Nav2
This is my params of Route Server
route_server:
ros__parameters:
base_frame: "base_footprint" # Robot's base frame
route_frame: "map" # Global reference frame
path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m)
max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible
max_planning_time: 2.0 # Maximum planning time (seconds)
costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources.
graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader
graph_file_loader.plugin:: nav2_route::GeoJsonGraphFileLoader # file loader plugin to use
# graph_filepath: '/workspaces/ros_jazzy_docker/nav2_ws/src/navigation2/nav2_route/graphs/turtlebot3_graph.geojson' # file path to graph to use
graph_filepath: $(find-pkg-share nav2_route)/graphs/turtlebot3_graph.geojson
edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"] # Edge scoring cost functions to use
DistanceScorer:
plugin: "nav2_route::DistanceScorer"
DynamicEdgesScorer:
plugin: "nav2_route::DynamicEdgesScorer"
operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use
AdjustSpeedLimit:
plugin: "nav2_route::AdjustSpeedLimit"
ReroutingService:
plugin: "nav2_route::ReroutingService"
tracker_update_rate: 50.0 # Rate at which to check the status of path tracking
aggregate_blocked_ids: false # Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently blocked IDs.
boundary_radius_to_achieve_node: 1.0 # Radius (m) near boundary nodes (e.g. start/end) to enable evaluation of achievement metric
radius_to_achieve_node: 2.0 # Radius (m) near route nodes as preliminary condition for evaluation of achievement metric
max_prune_dist_from_edge: 8.0 # Max distance from an edge to consider pruning it as in-progress (e.g. if we're too far away from the edge, its nonsensical to prune it)
min_prune_dist_from_goal: 0.15 # Min distance from goal node away from goal pose to consider goal node pruning as considering it as being passed (in case goal pose is very close to a goal node, but not exact)
min_prune_dist_from_start: 0.10 # Min distance from start node away from start pose to consider start node pruning as considering it as being passed (in case start pose is very close to a start node, but not exact)
prune_goal: true # Whether pruning the goal nodes from the route due to being past the goal pose requested is possible (pose requests only)
enable_nn_search: true # Whether to enable breadth first search considering the costmap to find the node closest to the start and edge poses, rather than using the euclidean nearest neighbor alone.
max_nn_search_iterations: 10000 # Maximum number of iterations for breadth-first search.
num_nearest_nodes: 5 # The number of nearest-neighbors to extract for breadth-first search to consider.
