Route Tool Rviz Panel

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -64,7 +64,7 @@ class BtServiceNode : public BT::ActionNodeBase
     getInput("service_name", service_name_);
     service_client_ = node_->create_client<ServiceT>(
       service_name_,
-      rclcpp::SystemDefaultsQoS(),
+      rmw_qos_profile_services_default,
       callback_group_);
 
     // Make a request for the service without parameter

nav2_bringup/params/nav2_params.yaml

@@ -316,6 +316,25 @@ waypoint_follower:
       enabled: True
       waypoint_pause_duration: 200
 
+route_server:
+  ros__parameters:
+    graph_filepath: "/home/steve/Documents/nav2_ws/src/navigation2/nav2_route/graphs/aws_graph.geojson"
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 1.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
 velocity_smoother:
   ros__parameters:
     smoothing_frequency: 20.0

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -62,13 +62,13 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   manager_srv_ = create_service<ManageLifecycleNodes>(
     get_name() + std::string("/manage_nodes"),
     std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
-    rclcpp::SystemDefaultsQoS(),
+    rmw_qos_profile_services_default,
     callback_group_);
 
   is_active_srv_ = create_service<std_srvs::srv::Trigger>(
     get_name() + std::string("/is_active"),
     std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
-    rclcpp::SystemDefaultsQoS(),
+    rmw_qos_profile_services_default,
     callback_group_);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;

nav2_route/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(${library_name} SHARED
   src/node_spatial_tree.cpp
   src/path_converter.cpp
   src/graph_loader.cpp
+  src/graph_saver.cpp
   src/goal_intent_extractor.cpp
 )
 
@@ -109,6 +110,14 @@ ament_target_dependencies(graph_file_loaders
     ${dependencies}
 )
 
+add_library(graph_file_savers SHARED
+  src/plugins/graph_file_savers/geojson_graph_file_saver.cpp  
+)
+
+ament_target_dependencies(graph_file_savers
+  ${dependencies}
+)
+
 pluginlib_export_plugin_description_file(nav2_route plugins.xml)
 
 install(DIRECTORY include/
@@ -119,7 +128,7 @@ install(TARGETS ${executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders
+install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -138,5 +147,5 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_dependencies(${dependencies})
-ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders)
+ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers)
 ament_package()

nav2_route/README.md

@@ -396,7 +396,8 @@ Note that there are parameters like `prune_goal`, `min_distance_from_start` and
 
 # Steve's TODO list
 
-- [ ] Sample files: AWS final + TB3 sandbox world
+- [ ] path marker points align with direction
+- [ ] Sample files: AWS final
 - [ ] QGIS demo + plugins for editing and visualizing graphs
 - [ ] use map for checking start/goal nodes for infra blockages not just NN. Evaluate K. Share costmap?
 
@@ -406,11 +407,12 @@ Note that there are parameters like `prune_goal`, `min_distance_from_start` and
   - Keep graph visualization in rviz but turn off by default
   - Default graph sandbox + launch configuration to set (and get filepath + mirror `map` comments). 
 
-  - Tutorials (bt change, plugin customize, file field examples)
+  - Tutorials (bt xml change, plugin customize (op/edge), file field examples)
   - BT XMLs (first/last mile, freq. replanning, navigation using it, WPF using it, clearance)
   - Demos (route -> global -> local. outdoor non-planar. waypoint follower (GPS?) of route nodes. controller server of dense path)
 
   - Finish system test with route file + evaluation
+  - stop / clearance
 
 - [ ] Testing by users
 

nav2_route/graphs/scripts/README.md

@@ -0,0 +1 @@
+These scripts are used to help automate parts of route graph generation process.  

nav2_route/graphs/scripts/export_shapefiles.py

@@ -0,0 +1,19 @@
+import geopandas as gpd
+import pandas as pd
+import sys
+from datetime import datetime
+
+try: 
+    file_prefix  = sys.argv[1] 
+    edges = gpd.read_file(sys.argv[2])
+    nodes = gpd.read_file(sys.argv[3])
+except: 
+    raise Exception("Incorrect arguements provided")
+
+now = datetime.now()
+
+graph = pd.concat([nodes, edges])
+
+file_name = file_prefix + "_" + now.strftime("%m_%d_%Y_%H_%M_%S") + ".geojson"
+
+graph.to_file(file_name, driver='GeoJSON')

nav2_route/graphs/scripts/generate_start_and_end_id.sql

@@ -0,0 +1,22 @@
+WITH start_points AS
+(
+	SELECT  n.id       AS node_id
+	       ,e.id       AS edge_id
+	       ,e.geometry AS edge_geometry
+	FROM edges AS e, nodes AS n
+	WHERE ST_INTERSECTS(n.geometry, ST_StartPoint(e.geometry)) 
+), end_points AS
+(
+	SELECT  n.id       AS node_id
+	       ,e.id       AS edge_id
+	       ,e.geometry AS edge_geometry
+	FROM edges AS e, nodes AS n
+	WHERE ST_INTERSECTS(n.geometry, ST_EndPoint(e.geometry)) 
+)
+SELECT  sp.edge_id       AS edge_id
+       ,sp.node_id       AS start_node
+       ,ep.node_id       AS end_node
+       ,sp.edge_geometry AS geometry
+FROM start_points AS sp
+JOIN end_points AS ep
+ON sp.edge_id = ep.edge_id

nav2_route/graphs/scripts/increment_edge_id.json

@@ -0,0 +1,21 @@
+{
+    "author": "josh",
+    "exported_at": "2023-03-15T19:27:03",
+    "expressions": [
+        {
+            "description": "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.0//EN\" \"http://www.w3.org/TR/REC-html40/strict.dtd\">\n<html><head><style type=\"text/css\">\np, li { white-space: pre-wrap; }\n</style></head><body style=\" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;\">\n<p style=\"-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;\"><br /></p></body></html>",
+            "expression": "if(maximum(\"id\") is null, 10000, maximum(\"id\")+1)",
+            "group": "user",
+            "name": "increment_edge_id",
+            "type": "expression"
+        },
+        {
+            "description": "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.0//EN\" \"http://www.w3.org/TR/REC-html40/strict.dtd\">\n<html><head><style type=\"text/css\">\np, li { white-space: pre-wrap; }\n</style></head><body style=\" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;\">\n<p style=\"-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;\"><br /></p></body></html>",
+            "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)",
+            "group": "user",
+            "name": "increment_node_id",
+            "type": "expression"
+        }
+    ],
+    "qgis_version": "3.22.4-Białowieża"
+}

nav2_route/graphs/scripts/increment_node_id.json

@@ -0,0 +1,14 @@
+{
+    "author": "josh",
+    "exported_at": "2023-03-15T19:22:14",
+    "expressions": [
+        {
+            "description": "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.0//EN\" \"http://www.w3.org/TR/REC-html40/strict.dtd\">\n<html><head><style type=\"text/css\">\np, li { white-space: pre-wrap; }\n</style></head><body style=\" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;\">\n<p style=\"-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;\"><br /></p></body></html>",
+            "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)",
+            "group": "user",
+            "name": "increment_node_id",
+            "type": "expression"
+        }
+    ],
+    "qgis_version": "3.22.4-Białowieża"
+}