diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 91b0c0cd9d8..bd81ee0ad48 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/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( service_name_, - rclcpp::SystemDefaultsQoS(), + rmw_qos_profile_services_default, callback_group_); // Make a request for the service without parameter diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index ffe076405f5..38ad8a262c7 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 2cbe22768c0..f18436f21f1 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -62,13 +62,13 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) manager_srv_ = create_service( 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( 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; diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index f4a4a477db9..b38ee848902 100644 --- a/nav2_route/CMakeLists.txt +++ b/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() diff --git a/nav2_route/README.md b/nav2_route/README.md index 2a68743b172..f2dd1189981 100644 --- a/nav2_route/README.md +++ b/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 diff --git a/nav2_route/graphs/scripts/README.md b/nav2_route/graphs/scripts/README.md new file mode 100644 index 00000000000..1bec9951d1e --- /dev/null +++ b/nav2_route/graphs/scripts/README.md @@ -0,0 +1 @@ +These scripts are used to help automate parts of route graph generation process. \ No newline at end of file diff --git a/nav2_route/graphs/scripts/export_shapefiles.py b/nav2_route/graphs/scripts/export_shapefiles.py new file mode 100644 index 00000000000..4addaac6ca4 --- /dev/null +++ b/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') \ No newline at end of file diff --git a/nav2_route/graphs/scripts/generate_start_and_end_id.sql b/nav2_route/graphs/scripts/generate_start_and_end_id.sql new file mode 100644 index 00000000000..09ec067b36c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/nav2_route/graphs/scripts/increment_edge_id.json b/nav2_route/graphs/scripts/increment_edge_id.json new file mode 100644 index 00000000000..8adb1c7e798 --- /dev/null +++ b/nav2_route/graphs/scripts/increment_edge_id.json @@ -0,0 +1,21 @@ +{ + "author": "josh", + "exported_at": "2023-03-15T19:27:03", + "expressions": [ + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 10000, maximum(\"id\")+1)", + "group": "user", + "name": "increment_edge_id", + "type": "expression" + }, + { + "description": "\n\n


", + "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" +} diff --git a/nav2_route/graphs/scripts/increment_node_id.json b/nav2_route/graphs/scripts/increment_node_id.json new file mode 100644 index 00000000000..ea0f5fe0dab --- /dev/null +++ b/nav2_route/graphs/scripts/increment_node_id.json @@ -0,0 +1,14 @@ +{ + "author": "josh", + "exported_at": "2023-03-15T19:22:14", + "expressions": [ + { + "description": "\n\n


", + "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" +} diff --git a/nav2_route/graphs/turtlebot3_world_graph.geojson b/nav2_route/graphs/turtlebot3_world_graph.geojson new file mode 100644 index 00000000000..c02ce85510f --- /dev/null +++ b/nav2_route/graphs/turtlebot3_world_graph.geojson @@ -0,0 +1,92 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 0.0, -1.7446 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ -2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.2554 ] } }, + { "type": "Feature", "properties": { "edge_id": 21, "start_node": 4, "end_node": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 22, "start_node": 7, "end_node": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 23, "start_node": 8, "end_node": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 24, "start_node": 16, "end_node": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 25, "start_node": 9, "end_node": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 26, "start_node": 5, "end_node": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 27, "start_node": 2, "end_node": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 28, "start_node": 6, "end_node": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 29, "start_node": 10, "end_node": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 30, "start_node": 17, "end_node": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 31, "start_node": 18, "end_node": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 32, "start_node": 11, "end_node": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 33, "start_node": 12, "end_node": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 34, "start_node": 14, "end_node": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 35, "start_node": 1, "end_node": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 36, "start_node": 4, "end_node": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 37, "start_node": 16, "end_node": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 38, "start_node": 15, "end_node": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 39, "start_node": 17, "end_node": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 40, "start_node": 10, "end_node": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 41, "start_node": 6, "end_node": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 42, "start_node": 2, "end_node": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 43, "start_node": 14, "end_node": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 44, "start_node": 12, "end_node": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 45, "start_node": 11, "end_node": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 46, "start_node": 18, "end_node": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 47, "start_node": 15, "end_node": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 48, "start_node": 8, "end_node": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 49, "start_node": 7, "end_node": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 50, "start_node": 4, "end_node": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 51, "start_node": 1, "end_node": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 52, "start_node": 5, "end_node": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 53, "start_node": 9, "end_node": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 54, "start_node": 16, "end_node": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 55, "start_node": 17, "end_node": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 56, "start_node": 9, "end_node": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 57, "start_node": 10, "end_node": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 58, "start_node": 6, "end_node": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 59, "start_node": 5, "end_node": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 60, "start_node": 1, "end_node": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 61, "start_node": 2, "end_node": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 62, "start_node": 5, "end_node": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 63, "start_node": 7, "end_node": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 64, "start_node": 9, "end_node": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 65, "start_node": 8, "end_node": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 66, "start_node": 10, "end_node": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 67, "start_node": 11, "end_node": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 68, "start_node": 12, "end_node": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 69, "start_node": 6, "end_node": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 70, "start_node": 3, "end_node": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 71, "start_node": 3, "end_node": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 72, "start_node": 1, "end_node": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 73, "start_node": 2, "end_node": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 74, "start_node": 11, "end_node": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 75, "start_node": 13, "end_node": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 76, "start_node": 12, "end_node": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 77, "start_node": 13, "end_node": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 78, "start_node": 16, "end_node": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 79, "start_node": 20, "end_node": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 80, "start_node": 17, "end_node": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 81, "start_node": 20, "end_node": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 82, "start_node": 8, "end_node": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 83, "start_node": 19, "end_node": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 84, "start_node": 7, "end_node": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "edge_id": 85, "start_node": 19, "end_node": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, 0.8554 ] ] ] } } + ] +} diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp new file mode 100644 index 00000000000..f22af6558df --- /dev/null +++ b/nav2_route/include/nav2_route/graph_saver.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__GRAPH_SAVER_HPP_ +#define NAV2_ROUTE__GRAPH_SAVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GraphSaver + * @brief An action server to save graph objects to a file + */ +class GraphSaver +{ +public: + /** + * @brief A constructor for nav2_route::GraphSaver + * @param node Lifecycle node encapsulated by the GraphSaver + * @param tf A tf buffer + * @param frame Coordinate frame that the graph belongs to + */ + explicit GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame); + + /** + * @brief A destructor for nav2_route::GraphSaver + */ + ~GraphSaver() = default; + + /** + * @brief Saves a graph object to a file + * @param graph Graph to save + * @param filepath The filepath to the graph data + * @return bool If successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath = ""); + +protected: + /** + * @brief Transform the coordinates in the graph to the route frame + * @param[in/out] graph The graph to be transformed + * @return True if transformation was successful + */ + bool transformGraph(Graph & graph); + + std::string route_frame_, graph_filepath_; + std::shared_ptr tf_; + rclcpp::Logger logger_{rclcpp::get_logger("GraphSaver")}; + + // Graph Parser + pluginlib::ClassLoader plugin_loader_; + GraphFileSaver::Ptr graph_file_saver_; + std::string default_plugin_id_; + std::string plugin_type_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GRAPH_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp new file mode 100644 index 00000000000..4851655e4dd --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ + +#include +#include + + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/types.hpp" + +namespace nav2_route +{ + +/** + * @class GraphFileSaver + * @brief A plugin interface to parse a file into the graph + */ +class GraphFileSaver +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + GraphFileSaver() = default; + + /** + * @brief Virtual destructor + */ + virtual ~GraphFileSaver() = default; + + /** + * @brief Configure the graph file saver, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief Method to save the graph to the filepath + * @param graph The graph to save + * @param filepath The path to save the file to + * @return true if graph was successfully saved + */ + virtual bool saveGraphToFile( + Graph & graph, + std::string filepath) = 0; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp new file mode 100644 index 00000000000..13e9e51a34a --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp @@ -0,0 +1,106 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" + +#ifndef NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::GeoJsonGraphFileSaver + * @brief A GraphFileSaver plugin to save a geojson graph representation + */ +class GeoJsonGraphFileSaver : public GraphFileSaver +{ +public: + using Json = nlohmann::json; + + /** + * @brief Constructor + */ + GeoJsonGraphFileSaver() = default; + + /** + * @brief Destructor + */ + ~GeoJsonGraphFileSaver() = default; + + /** + * @brief Configure, but do not store the node + * @param parent pointer to user's node + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Saves the graph to a geojson file + * @param graph The graph to save to the geojson file + * @param filepath The path to save the graph to + * @return True if successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath) override; + +protected: + /** + * @brief Add nodes into the graph + * @param[out] graph The graph that will contain the new nodes + * @param[in] json_features Json array to add the nodes to + */ + void loadNodesFromGraph(Graph & graph, std::vector & json_features); + + /** + * @brief Add edges into the graph + * @param[out] graph The graph that will contain the new edges + * @param[in] json_edges Json array to add the edges to + */ + void loadEdgesFromGraph(Graph & graph, std::vector & json_edges); + + /** + * @brief Convert graph metadata to Json + * @param metadata Metadata from a node or edge in the graph + * @param json_metadata Json entry to store metadata in + */ + void convertMetaDataToJson(const Metadata & metadata, Json & json_metadata); + + /** + * @brief Convert graph operation to Json + * @param Operations Operations information from the graph + * @param json_operations Json entry to store operation data in + */ + void convertOperationsToJson(const Operations & operations, Json & json_operations); + + rclcpp::Logger logger_{rclcpp::get_logger("GeoJsonGraphFileSaver")}; +}; + +NLOHMANN_JSON_SERIALIZE_ENUM( + OperationTrigger, { + {OperationTrigger::NODE, "NODE"}, + {OperationTrigger::ON_ENTER, "ON_ENTER"}, + {OperationTrigger::ON_EXIT, "ON_EXIT"}, + }) + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp index 31e77f38f42..672108729ee 100644 --- a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp +++ b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp @@ -122,7 +122,7 @@ class RouteOperationClient : public RouteOperation // indicate the endpoint for the particular service call. if (!main_srv_name_.empty()) { main_client_ = node->create_client( - main_srv_name_, rclcpp::SystemDefaultsQoS(), callback_group_); + main_srv_name_, rmw_qos_profile_services_default, callback_group_); } } @@ -163,7 +163,7 @@ class RouteOperationClient : public RouteOperation } else { auto node = node_.lock(); auto client = node->create_client( - srv_name, rclcpp::SystemDefaultsQoS(), callback_group_); + srv_name, rmw_qos_profile_services_default, callback_group_); response = callService(client, req); } diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp index 99cb79649c6..7ad875c8aef 100644 --- a/nav2_route/include/nav2_route/types.hpp +++ b/nav2_route/include/nav2_route/types.hpp @@ -61,6 +61,7 @@ typedef Node * NodePtr; typedef std::vector NodeVector; typedef NodeVector Graph; typedef std::unordered_map GraphToIDMap; +typedef std::unordered_map> GraphToIncomingEdgesMap; typedef std::vector NodePtrVector; typedef std::pair NodeElement; typedef std::pair NodeExtents; diff --git a/nav2_route/media/architecture.png b/nav2_route/media/architecture.png index f3a2a1bb6ab..3bab56db5e6 100644 Binary files a/nav2_route/media/architecture.png and b/nav2_route/media/architecture.png differ diff --git a/nav2_route/plugins.xml b/nav2_route/plugins.xml index 946c2eaee59..fbb0d9941b1 100644 --- a/nav2_route/plugins.xml +++ b/nav2_route/plugins.xml @@ -51,6 +51,11 @@ Parse the geojson graph file into the graph data type + + + Save a route graph to a geojson graph file + + diff --git a/nav2_route/src/graph_saver.cpp b/nav2_route/src/graph_saver.cpp new file mode 100644 index 00000000000..f2a3490ed1c --- /dev/null +++ b/nav2_route/src/graph_saver.cpp @@ -0,0 +1,141 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_route/graph_saver.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_route +{ + +GraphSaver::GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame) +: plugin_loader_("nav2_route", "nav2_route::GraphFileSaver"), + default_plugin_id_("GeoJsonGraphFileSaver") +{ + logger_ = node->get_logger(); + tf_ = tf; + route_frame_ = frame; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + graph_filepath_ = node->get_parameter("graph_filepath").as_string(); + + // Default Graph Parser + const std::string default_plugin_type = "nav2_route::GeoJsonGraphFileSaver"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_saver", rclcpp::ParameterValue(default_plugin_id_)); + auto graph_file_saver_id = node->get_parameter("graph_file_saver").as_string(); + if (graph_file_saver_id == default_plugin_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_id_ + ".plugin", rclcpp::ParameterValue(default_plugin_type)); + } + + // Create graph file saver plugin + try { + plugin_type_ = nav2_util::get_plugin_type_param(node, graph_file_saver_id); + graph_file_saver_ = plugin_loader_.createSharedInstance((plugin_type_)); + RCLCPP_INFO( + logger_, "Created GraphFileSaver %s of type %s", + graph_file_saver_id.c_str(), plugin_type_.c_str()); + graph_file_saver_->configure(node); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create GraphFileSaver. Exception: %s", ex.what()); + throw ex; + } +} + +bool GraphSaver::saveGraphToFile( + Graph & graph, + std::string filepath) +{ + if (filepath.empty() && !graph_filepath_.empty()) { + RCLCPP_DEBUG( + logger_, "The graph filepath was not provided. " + "Setting to %s", graph_filepath_.c_str()); + filepath = graph_filepath_; + } else if (filepath.empty() && graph_filepath_.empty()) { + // No graph to try to save + RCLCPP_WARN( + logger_, + "The graph filepath was not provided and no default was specified. " + "Failed to save the route graph."); + return false; + } + + RCLCPP_INFO( + logger_, + "Saving graph file from %s, by parser %s", filepath.c_str(), plugin_type_.c_str()); + + if (!graph_file_saver_->saveGraphToFile(graph, filepath)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", filepath.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphSaver::transformGraph(Graph & graph) +{ + std::unordered_map cached_transforms; + for (auto & node : graph) { + std::string node_frame = node.coords.frame_id; + if (node_frame.empty() || node_frame == route_frame_) { + // If there is no frame provided or the frame of the node is the same as the route graph + // then no transform is required + continue; + } + // Find the transform to convert node from node frame to route frame + if (cached_transforms.find(node_frame) == cached_transforms.end()) { + tf2::Transform tf_transform; + bool got_transform = nav2_util::getTransform( + node_frame, route_frame_, tf2::durationFromSec(0.1), tf_, tf_transform); + + if (!got_transform) { + RCLCPP_WARN(logger_, + "Could not get transform from node frame %s to route frame %s", + node_frame.c_str(), route_frame_.c_str()); + return false; + } + + cached_transforms.insert({node_frame, tf_transform}); + } + // Apply the transform + tf2::Vector3 graph_coord( + node.coords.x, + node.coords.y, + 0.0); + + tf2::Vector3 new_coord = cached_transforms[node_frame] * graph_coord; + + node.coords.x = static_cast(new_coord.x()); + node.coords.y = static_cast(new_coord.y()); + node.coords.frame_id = route_frame_; + } + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp new file mode 100644 index 00000000000..5a2bd7b3ad4 --- /dev/null +++ b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp" + +namespace nav2_route +{ + +void GeoJsonGraphFileSaver::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO(node->get_logger(), "Configuring geojson graph file saver"); + logger_ = node->get_logger(); +} + +bool GeoJsonGraphFileSaver::saveGraphToFile( + Graph & graph, std::string filepath) +{ + Json json_graph, json_crs, json_properties; + json_graph["type"] = "FeatureCollection"; + json_graph["name"] = "graph"; + json_properties["name"] = "urn:ogc:def:crs:EPSG::3857"; + json_crs["type"] = "name"; + json_crs["properties"] = json_properties; + json_graph["crs"] = json_crs; + std::vector json_features; + try { + loadNodesFromGraph(graph, json_features); + loadEdgesFromGraph(graph, json_features); + json_graph["features"] = json_features; + std::ofstream file(filepath); + file << json_graph.dump(4) << std::endl; + file.close(); + } catch (const std::ios_base::failure & e) { + RCLCPP_ERROR(logger_, "Error: %s", e.what()); + } catch (const std::bad_alloc& e) { + RCLCPP_ERROR(logger_, "Error allocating memory to save the GEOJson file"); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "An error occured: %s", e.what()); + } + return true; +} + +void GeoJsonGraphFileSaver::loadNodesFromGraph( + Graph & graph, std::vector & json_features) +{ + for (const auto & node : graph) { + Json json_feature, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "Point"; + json_geometry["coordinates"] = std::vector{node.coords.x, node.coords.y}; + json_feature["geometry"] = json_geometry; + json_properties["id"] = node.nodeid; + json_properties["frame"] = node.coords.frame_id; + convertMetaDataToJson(node.metadata, json_metadata); + if (json_metadata.size()) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(node.operations, json_operations); + if (json_operations.size()) { + json_properties["operations"] = json_operations; + } + json_feature["properties"] = json_properties; + json_feature["type"] = "Feature"; + json_features.push_back(json_feature); + } +} + +void GeoJsonGraphFileSaver::loadEdgesFromGraph( + Graph & graph, std::vector & json_edges) +{ + for (const auto & node : graph) { + for (const auto & edge : node.neighbors) { + Json json_edge, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "MultiLineString"; + json_edge["geometry"] = json_geometry; + json_properties["id"] = edge.edgeid; + json_properties["startid"] = edge.start->nodeid; + json_properties["endid"] = edge.end->nodeid; + convertMetaDataToJson(edge.metadata, json_metadata); + if (json_metadata.size()) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(edge.operations, json_operations); + if (json_operations.size()) { + json_properties["operations"] = json_operations; + } + json_edge["properties"] = json_properties; + json_edge["type"] = "Feature"; + if (edge.edge_cost.cost != 0.0) { + json_edge["cost"] = edge.edge_cost.cost; + } + json_edges.push_back(json_edge); + } + } +} + +void GeoJsonGraphFileSaver::convertMetaDataToJson( + const Metadata & metadata, Json & json_metadata) +{ + /* Function partially created using GPT */ + for (auto itr = metadata.data.begin(); itr != metadata.data.end(); itr++) { + if (itr->second.type() == typeid(std::string)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(unsigned int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(float)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(bool)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(std::nullptr_t)) { + json_metadata[itr->first] = nullptr; + } else if (itr->second.type() == typeid(Metadata)) { + // If the itr->second is another Metadata, recursively convert it to JSON + Json nested_metadata_json; + convertMetaDataToJson(std::any_cast(itr->second), nested_metadata_json); + json_metadata[itr->first] = nested_metadata_json; + } else if (itr->second.type() == typeid(std::vector)) { + // If the itr->second is a vector, convert each element + std::vector arrayJson; + for (const auto& element : std::any_cast>(itr->second)) { + if (element.type() == typeid(std::string)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(unsigned int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(float)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(bool)) { + arrayJson.push_back(std::any_cast(element)); + } + } + json_metadata[itr->first] = arrayJson; + } else { + // If we have an unknown type, handle as needed + json_metadata[itr->first] = itr->second.type().name(); + } + } +} + +void GeoJsonGraphFileSaver::convertOperationsToJson( + const Operations & operations, Json & json_operations) +{ + for (const auto & operation : operations) { + Json json_operation, json_metadata; + json_operation["type"] = operation.type; + json_operation["trigger"] = operation.trigger; + convertMetaDataToJson(operation.metadata, json_metadata); + if (json_metadata.size()) { + json_operation["metadata"] = json_metadata; + } + json_operations[operation.type] = json_operation; + } +} +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GeoJsonGraphFileSaver, nav2_route::GraphFileSaver) diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 86c1a19d923..533011ea854 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -323,7 +323,8 @@ void RouteServer::setRouteGraph( std::shared_ptr response) { RCLCPP_INFO(get_logger(), "Setting new route graph: %s.", request->graph_filepath.c_str()); - + graph_.clear(); + id_to_graph_map_.clear(); try { if (!graph_loader_->loadGraphFromFile(graph_, id_to_graph_map_, request->graph_filepath)) { RCLCPP_WARN( diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 39bdbabae49..64daaa3e5c7 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(nav2_util REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(nav2_route REQUIRED) find_package(pluginlib REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(rclcpp REQUIRED) @@ -41,6 +42,15 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) +# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTORCC ON) + +qt5_wrap_ui(route_tool_UIS_H resource/route_tool.ui) +qt5_wrap_cpp(route_tool_MOCS include/nav2_rviz_plugins/route_tool.hpp) + include_directories( include ) @@ -50,9 +60,12 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/goal_tool.cpp src/nav2_panel.cpp + src/route_tool.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_headers_to_moc} + ${route_tool_UIS_H} + ${route_tool_MOCS} ) set(dependencies @@ -61,6 +74,7 @@ set(dependencies nav2_lifecycle_manager nav2_msgs nav_msgs + nav2_route pluginlib Qt5 rclcpp @@ -107,6 +121,8 @@ install( DESTINATION include/ ) +install(DIRECTORY rviz launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -120,6 +136,7 @@ ament_export_dependencies( geometry_msgs map_msgs nav_msgs + nav2_route rclcpp ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp new file mode 100644 index 00000000000..763a65bf3bc --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -0,0 +1,126 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef rviz_panel_H_ +#define rviz_panel_H_ + +#include +#include +#include +#include + +/** + * Include header generated from ui file + * Note that you will need to use add_library function first + * in order to generate the header file from ui. + */ +#include + +// Other ROS dependencies +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace nav2_rviz_plugins +{ + /** + * Here we declare our new subclass of rviz::Panel. Every panel which + * can be added via the Panels/Add_New_Panel menu is a subclass of + * rviz::Panel. + */ + + class RouteTool : public rviz_common::Panel + { + /** + * This class uses Qt slots and is a subclass of QObject, so it needs + * the Q_OBJECT macro. + */ + Q_OBJECT + + public: + /** + * QWidget subclass constructors usually take a parent widget + * parameter (which usually defaults to 0). At the same time, + * pluginlib::ClassLoader creates instances by calling the default + * constructor (with no arguments). Taking the parameter and giving + * a default of 0 lets the default constructor work and also lets + * someone using the class for something else to pass in a parent + * widget as they normally would with Qt. + */ + RouteTool(QWidget * parent = nullptr); + + void onInitialize() override; + + /** + * Now we declare overrides of rviz_common::Panel functions for saving and + * loading data from the config file. Here the data is the topic name. + */ + virtual void save(rviz_common::Config config) const; + virtual void load(const rviz_common::Config & config); + + + /** + * Here we declare some internal slots. + */ + private Q_SLOTS: + void on_load_button_clicked(void); + + void on_save_button_clicked(void); + + void on_create_button_clicked(void); + + void on_confirm_button_clicked(void); + + void on_delete_button_clicked(void); + + void on_add_node_button_toggled(void); + + void on_edit_node_button_toggled(void); + + /** + * Finally, we close up with protected member variables + */ + protected: + // UI pointer + std::unique_ptr ui_; + + private: + void update_route_graph(void); + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr graph_loader_; + std::shared_ptr graph_saver_; + std::shared_ptr tf_; + nav2_route::Graph graph_; + nav2_route::GraphToIDMap graph_to_id_map_; + nav2_route::GraphToIDMap edge_to_node_map_; + nav2_route::GraphToIncomingEdgesMap graph_to_incoming_edges_map_; + + rclcpp::Publisher::SharedPtr + graph_vis_publisher_; + rclcpp::Subscription::SharedPtr + clicked_point_subscription_; + + unsigned int next_node_id_ = 0; + + + }; +} // namespace nav2_rviz_plugins +#endif diff --git a/nav2_rviz_plugins/launch/route_tool.launch.py b/nav2_rviz_plugins/launch/route_tool.launch.py new file mode 100644 index 00000000000..02d8ca5e204 --- /dev/null +++ b/nav2_rviz_plugins/launch/route_tool.launch.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 John Chrosniak +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import launch_ros.actions +import os + + +def generate_launch_description(): + + + # Nodes launching commands + map_file = LaunchConfiguration("yaml_filename") + + declare_map_file_cmd = DeclareLaunchArgument( + 'yaml_filename', + default_value='', + description='Full path to an occupancy grid map yaml file') + + start_route_tool_cmd = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['-d' + os.path.join(get_package_share_directory('nav2_rviz_plugins'), 'rviz', 'route_tool.rviz')]) + + start_map_server = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[{'yaml_filename' : map_file}] + ) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'use_sim_time': False}, + {'autostart': True}, + {'node_names': ["map_server"]}]) + + + + return LaunchDescription([ + declare_map_file_cmd, + start_route_tool_cmd, + start_map_server, + start_lifecycle_manager_cmd + ]) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5e090d7496f..1a24fd2db2b 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -14,6 +14,7 @@ nav2_util nav2_lifecycle_manager nav2_msgs + nav2_route nav_msgs pluginlib rclcpp diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 197a9a750d7..799a4fe4186 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -18,4 +18,10 @@ The Particle Cloud rviz display. + + A tool used to create route graphs. + + diff --git a/nav2_rviz_plugins/resource/route_tool.ui b/nav2_rviz_plugins/resource/route_tool.ui new file mode 100644 index 00000000000..31b31bf3c06 --- /dev/null +++ b/nav2_rviz_plugins/resource/route_tool.ui @@ -0,0 +1,386 @@ + + + + + + route_tool + + + + 0 + 0 + 394 + 461 + + + + MainWindow + + + + + + + + + + 0 + + + + + 0 + 0 + + + + Add + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + Text: + + + + + + + + + + + + + + 16777215 + 50 + + + + Field 1: + + + + + + + + 500 + 30 + + + + + + + + + + + + + 16777215 + 50 + + + + Field 2: + + + + + + + + 16777215 + 30 + + + + + + + + + + + + Create + + + + + + + + + + Edit + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Text: + + + + + + + + + + + + + Field 1: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Field 2: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Confirm + + + + + + + + + + + + + Remove + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + Delete + + + + + + + + + + + + + + + + + + + + Load + + + + + + + Save + + + + + + + + + + + + + + + diff --git a/nav2_rviz_plugins/rviz/route_tool.rviz b/nav2_rviz_plugins/rviz/route_tool.rviz new file mode 100644 index 00000000000..244d39685d3 --- /dev/null +++ b/nav2_rviz_plugins/rviz/route_tool.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Route Graph1/Topic1 + - /Map1 + - /Map1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 305 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: nav2_rviz_plugins/Route Tool + Name: Route Tool +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Route Graph + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /route_graph + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 22.106815338134766 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1003 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000034dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120072006f0075007400650054006f006f006c00000001a4000001e60000006e00fffffffb000000140052006f00750074006500200054006f006f006c01000001ff0000018b0000018b00ffffff000000010000010f0000034dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000034d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000034d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Route Tool: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 27 + routeTool: + collapsed: false diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp new file mode 100644 index 00000000000..5af5115abaf --- /dev/null +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -0,0 +1,273 @@ +// Copyright (c) 2024 John Chrosniak +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/route_tool.hpp" +#include "rviz_common/display_context.hpp" +#include +#include +#include +#include +#include +#include +#include + + +namespace nav2_rviz_plugins +{ + RouteTool::RouteTool(QWidget * parent) + : rviz_common::Panel(parent), + ui_(std::make_unique()) + { + // Extend the widget with all attributes and children from UI file + ui_->setupUi(this); + node_ = std::make_shared("route_tool_node", "", rclcpp::NodeOptions()); + node_->configure(); + graph_vis_publisher_ = node_->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + node_->activate(); + tf_ = std::make_shared(node_->get_clock()); + graph_loader_ = std::make_shared(node_, tf_, "map"); + graph_saver_ = std::make_shared(node_, tf_, "map"); + ui_->add_node_button->setChecked(true); + ui_->edit_node_button->setChecked(true); + ui_->remove_node_button->setChecked(true); + } + + void RouteTool::onInitialize(void) { + auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + clicked_point_subscription_ = node->create_subscription( + "clicked_point", 1, [this](const geometry_msgs::msg::PointStamped::SharedPtr msg) { + ui_->add_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->add_field_2->setText(std::to_string(msg->point.y).c_str()); + ui_->edit_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->edit_field_2->setText(std::to_string(msg->point.y).c_str()); + }); + } + + void RouteTool::on_load_button_clicked(void) + { + graph_to_id_map_.clear(); + edge_to_node_map_.clear(); + graph_to_incoming_edges_map_.clear(); + graph_.clear(); + struct passwd *pw = getpwuid(getuid()); + std::string homedir(pw->pw_dir); + QString filename = QFileDialog::getOpenFileName(this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + graph_loader_->loadGraphFromFile(graph_, graph_to_id_map_, filename.toStdString()); + unsigned int max_node_id = 0; + for (const auto & node : graph_) { + max_node_id = std::max(node.nodeid, max_node_id); + for (const auto & edge : node.neighbors) { + max_node_id = std::max(edge.edgeid, max_node_id); + edge_to_node_map_[edge.edgeid] = node.nodeid; + if (graph_to_incoming_edges_map_.find(edge.end->nodeid) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[edge.end->nodeid].push_back(edge.edgeid); + } else { + graph_to_incoming_edges_map_[edge.end->nodeid] = std::vector {edge.edgeid}; + } + } + } + next_node_id_ = max_node_id + 1; + update_route_graph(); + } + + void RouteTool::on_save_button_clicked(void) + { + struct passwd *pw = getpwuid(getuid()); + std::string homedir(pw->pw_dir); + QString filename = QFileDialog::getSaveFileName(this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + RCLCPP_INFO(node_->get_logger(), "Save graph to: %s", filename.toStdString().c_str()); + graph_saver_->saveGraphToFile(graph_, filename.toStdString()); + } + + void RouteTool::on_create_button_clicked(void) + { + if (ui_->add_field_1->toPlainText() == "" || ui_->add_field_2->toPlainText() == "") return; + if (ui_->add_node_button->isChecked()) { + auto longitude = ui_->add_field_1->toPlainText().toFloat(); + auto latitude = ui_->add_field_2->toPlainText().toFloat(); + nav2_route::Node new_node; + new_node.nodeid = next_node_id_; + new_node.coords.x = longitude; + new_node.coords.y = latitude; + graph_.push_back(new_node); + graph_to_id_map_[next_node_id_++] = graph_.size() - 1; + RCLCPP_INFO(node_->get_logger(), "Adding node at: (%f, %f)", longitude, latitude); + update_route_graph(); + } else if (ui_->add_edge_button->isChecked()) { + auto start_node = ui_->add_field_1->toPlainText().toInt(); + auto end_node = ui_->add_field_2->toPlainText().toInt(); + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[start_node]].addEdge(edge_cost, &(graph_[graph_to_id_map_[end_node]]), next_node_id_); + if (graph_to_incoming_edges_map_.find(end_node) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[end_node].push_back(next_node_id_); + } else { + graph_to_incoming_edges_map_[end_node] = std::vector {next_node_id_}; + } + edge_to_node_map_[next_node_id_++] = start_node; + RCLCPP_INFO(node_->get_logger(), "Adding edge from %d to %d", start_node, end_node); + update_route_graph(); + } + ui_->add_field_1->setText(""); + ui_->add_field_2->setText(""); + } + + void RouteTool::on_confirm_button_clicked(void) + { + if (ui_->edit_id->toPlainText() == "" || ui_->edit_field_1->toPlainText() == "" || ui_->edit_field_2->toPlainText() == "") return; + if (ui_->edit_node_button->isChecked()) { + auto node_id = ui_->edit_id->toPlainText().toInt(); + auto new_longitude = ui_->edit_field_1->toPlainText().toFloat(); + auto new_latitude = ui_->edit_field_2->toPlainText().toFloat(); + if (graph_to_id_map_.find(node_id) != graph_to_id_map_.end()) + { + graph_[graph_to_id_map_[node_id]].coords.x = new_longitude; + graph_[graph_to_id_map_[node_id]].coords.y = new_latitude; + update_route_graph(); + } + } else if (ui_->edit_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->edit_id->toPlainText().toInt(); + auto new_start = ui_->edit_field_1->toPlainText().toInt(); + auto new_end = ui_->edit_field_2->toPlainText().toInt(); + // Find and remove current edge + auto current_start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = current_start_node->neighbors.begin(); itr != current_start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + current_start_node->neighbors.erase(itr); + break; + } + } + // Create new edge with same ID using new start and stop nodes + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[new_start]].addEdge(edge_cost, &(graph_[graph_to_id_map_[new_end]]), edge_id); + edge_to_node_map_[edge_id] = new_start; + if (graph_to_incoming_edges_map_.find(new_end) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[new_end].push_back(edge_id); + } else { + graph_to_incoming_edges_map_[new_end] = std::vector {edge_id}; + } + update_route_graph(); + } + ui_->edit_id->setText(""); + ui_->edit_field_1->setText(""); + ui_->edit_field_2->setText(""); + } + + void RouteTool::on_delete_button_clicked(void) + { + if ( ui_->remove_id->toPlainText() == "") return; + if (ui_->remove_node_button->isChecked()) { + unsigned int node_id = ui_->remove_id->toPlainText().toInt(); + // Remove edges pointing to the removed node + for (auto edge_id : graph_to_incoming_edges_map_[node_id]) { + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + } + if (graph_[graph_to_id_map_[node_id]].nodeid == node_id) { + graph_.erase(graph_.begin() + graph_to_id_map_[node_id]); + // Need to adjust pointers and idx map since indices in the vector changed + for (auto idx = graph_to_id_map_[node_id]; idx < graph_.size(); idx++) { + auto& moved_node = graph_[idx]; + graph_to_id_map_[moved_node.nodeid]--; + } + for (auto idx = graph_to_id_map_[node_id]; idx < graph_.size(); idx++) { + auto& moved_node = graph_[idx]; + for (auto& edge : moved_node.neighbors) { + edge.start = &moved_node; + } + for (auto edge_id : graph_to_incoming_edges_map_[moved_node.nodeid]) { + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + itr->end = &moved_node; + } + } + } + } + graph_to_id_map_.erase(node_id); + graph_to_incoming_edges_map_.erase(node_id); + RCLCPP_INFO(node_->get_logger(), "Removed node %d", node_id); + } + update_route_graph(); + } else if (ui_->remove_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->remove_id->toPlainText().toInt(); + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + RCLCPP_INFO(node_->get_logger(), "Removed edge %d", edge_id); + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + update_route_graph(); + } + ui_->remove_id->setText(""); + } + + void RouteTool::on_add_node_button_toggled(void) + { + if (ui_->add_node_button->isChecked()) { + ui_->add_text->setText("Position:"); + ui_->add_label_1->setText("X:"); + ui_->add_label_2->setText("Y:"); + } else { + ui_->add_text->setText("Connections:"); + ui_->add_label_1->setText("Start Node ID:"); + ui_->add_label_2->setText("End Node ID:"); + + } + } + + void RouteTool::on_edit_node_button_toggled(void) { + if (ui_->edit_node_button->isChecked()) { + ui_->edit_text->setText("Position:"); + ui_->edit_label_1->setText("X:"); + ui_->edit_label_2->setText("Y:"); + } else { + ui_->edit_text->setText("Connections:"); + ui_->edit_label_1->setText("Start Node ID:"); + ui_->edit_label_2->setText("End Node ID:"); + } + } + + void RouteTool::update_route_graph(void) + { + graph_vis_publisher_->publish(nav2_route::utils::toMsg(graph_, "map", node_->now())); + } + + void RouteTool::save(rviz_common::Config config) const + { + rviz_common::Panel::save(config); + } + + void RouteTool::load(const rviz_common::Config & config) + { + rviz_common::Panel::load(config); + } +} // namespace nav2_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::RouteTool, rviz_common::Panel) \ No newline at end of file diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e65b2f928d0..903c2bb2868 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -105,20 +105,21 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - add_subdirectory(src/behavior_tree) - add_subdirectory(src/planning) - add_subdirectory(src/localization) - add_subdirectory(src/system) - add_subdirectory(src/system_failure) - add_subdirectory(src/updown) - add_subdirectory(src/waypoint_follower) - add_subdirectory(src/behaviors/spin) - add_subdirectory(src/behaviors/wait) - add_subdirectory(src/behaviors/backup) - add_subdirectory(src/behaviors/drive_on_heading) - add_subdirectory(src/behaviors/assisted_teleop) - add_subdirectory(src/costmap_filters) - add_subdirectory(src/error_codes) + #add_subdirectory(src/behavior_tree) + #add_subdirectory(src/planning) + #add_subdirectory(src/localization) + #add_subdirectory(src/system) + #add_subdirectory(src/system_failure) + #add_subdirectory(src/updown) + #add_subdirectory(src/waypoint_follower) + add_subdirectory(src/route) + #add_subdirectory(src/behaviors/spin) + #add_subdirectory(src/behaviors/wait) + #add_subdirectory(src/behaviors/backup) + #add_subdirectory(src/behaviors/drive_on_heading) + #add_subdirectory(src/behaviors/assisted_teleop) + #add_subdirectory(src/costmap_filters) + #add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/src/route/CMakeLists.txt b/nav2_system_tests/src/route/CMakeLists.txt new file mode 100644 index 00000000000..a0d07cfcc89 --- /dev/null +++ b/nav2_system_tests/src/route/CMakeLists.txt @@ -0,0 +1,16 @@ +ament_add_test(test_route + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_route_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=tester_node.py + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner/NavfnPlanner +) diff --git a/nav2_system_tests/src/route/README.md b/nav2_system_tests/src/route/README.md new file mode 100644 index 00000000000..a9888fa93ef --- /dev/null +++ b/nav2_system_tests/src/route/README.md @@ -0,0 +1,5 @@ +This is a series of tests using the python3 simple commander API to test the route server in a practical routing and tracking task. + +TODO +- controller to use (needs testing or pulling in MPPI) +- route file TB3 diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py new file mode 100644 index 00000000000..3678b242ac8 --- /dev/null +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if (os.getenv('ASTAR') == 'True'): + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + new_yaml = configured_params.perform(context) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', '-2.0', '-0.5', '0.0', '2.0', + '-e', 'True'], + name='tester_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py new file mode 100644 index 00000000000..70aec503e7e --- /dev/null +++ b/nav2_system_tests/src/route/tester_node.py @@ -0,0 +1,362 @@ +#! /usr/bin/env python3 +# Copyright 2023 Samsung Research +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import math +import sys +import time + +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputeRoute, ComputeAndTrackRoute +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class RouteTester(Node): + + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.compute_action_client = ActionClient(self, ComputeRoute, 'compute_route') + self.compute_track_action_client = ActionClient(self, ComputeAndTrackRoute, 'compute_and_track_route') + self.feedback_msgs = [] + + def runComputeRouteTest(self): + # Test 1: See if we can compute a route that is valid and correctly sized + self.info_msg("Waiting for 'ComputeRoute' action server") + while not self.compute_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeRoute' action server not available, waiting...") + + route_msg = ComputeRoute.Goal() + route_msg.start = self.getStampedPoseMsg(self.initial_pose) + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = True + route_msg.use_poses = True + + self.info_msg('Sending ComputeRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeRoute' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + result = get_result_future.result().result + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + self.info_msg('Action completed! Checking validity of results...') + + # Check result for validity + assert(len(result.path) > 0) + assert(result.route.route_cost > 0) # TODO for actual values expected + assert(len(result.route.nodes) > 0) # TODO for actual values expected + assert(len(result.route.edge_ids) > 0) # TODO for actual values expected + assert(result.error_code == 0) + + self.info_msg('Goal succeeded!') + return True + + def runTrackRouteTest(self): + # Test 1: See if we can compute and track a route with proper state + self.info_msg("Waiting for 'ComputeAndTrackRoute' action server") + while not self.compute_track_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeAndTrackRoute' action server not available, waiting...") + + route_msg = ComputeAndTrackRoute.Goal() + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = False # Use TF pose instead + route_msg.use_poses = True + + self.info_msg('Sending ComputeAndTrackRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeAndTrackRoute' action to complete") + progressing = True + last_feedback_msg = None + while progressing: + rclpy.spin_until_future_complete(self, get_result_future, timeout_sec = 0.10) + status = get_result_future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + progressing = False + elif status == GoalStatus.STATUS_CANCELED or status == GoalStatus.STATUS_ABORTED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + # Else, processing. Check feedback + while len(self.feedback_msgs) > 0: + feedback_msg = self.feedback_msgs.pop(0) + assert(len(feedback_msg.path) > 0) + assert(feedback_msg.route.route_cost > 0) # TODO for actual values expected + assert(len(feedback_msg.route.nodes) > 0) # TODO for actual values expected + assert(len(feedback_msg.route.edge_ids) > 0) # TODO for actual values expected + assert(len(feedback_msg.operations_triggered) == 1) + assert(feedback_msg.operations_triggered[0] == "ReroutingService") + + if last_feedback_msg and last_feedback_msg != feedback_msg and last_feedback_msg.route == False: + if last_feedback_msg.next_node_id != feedback_msg.last_node_id: + self.error_msg('Feedback state is not tracking in order!') + return False + if feedback_msg.current_edge_id == 0: + self.error_msg('Feedback state does not contain the proper edge info!') + return False + + last_feedback_msg = feedback_msg + + result = get_result_future.result().result + + if int(last_feedback_msg.next_node_id) != int(last_feedback.route.nodes[-1].nodeid): + self.error_msg('Terminal feedback state is not correct!') + return False + + self.info_msg('Action completed! Checking validity of terminal condition...') + + # Check result for validity + if not self.distanceFromGoal() < 0.5: + self.error_msg('Did not make it to the goal pose!') + return False + + self.info_msg('Goal succeeded!') + return True + + def feedback_callback(self, feedback_msg): + self.feedback_msgs.append(feedback_msg) + + def distanceFromGoal(self): + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg(f'Distance from goal is: {distance}') + return distance + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{node_service} service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while state != 'active': + self.info_msg(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg(f'Result of get_state: {state}') + else: + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester): + # set transforms to use_sim_time + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runComputeRouteTest() + result = result and robot_tester.runTrackRouteTest() + + if result: + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='Route server tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create test object + init_x, init_y, final_x, final_y = args.robot[0] + tester = RouteTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) + tester.info_msg( + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + ' via route server.' + ) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + # run tests + passed = run_all_tests(tester) + + tester.shutdown() + testers[0].info_msg('Done Shutting Down.') + + if passed != expect_failure: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 62e1b58838c..9be5912e8c9 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -46,7 +46,7 @@ class ServiceClient callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); client_ = node_->create_client( service_name, - rclcpp::SystemDefaultsQoS(), + rmw_qos_profile_services_default, callback_group_); } diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 49b32e4aa4d..fc4aee5c5e3 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -34,7 +34,7 @@ #include "nav2_core/waypoint_task_executor.hpp" #include "opencv4/opencv2/core.hpp" #include "opencv4/opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.hpp" +#include "cv_bridge/cv_bridge.h" #include "image_transport/image_transport.hpp"