Route Tool Rviz Panel

nav2_route/CMakeLists.txt

@@ -39,6 +39,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
 )
 
@@ -187,6 +188,37 @@ target_link_libraries(graph_file_loaders PRIVATE
   tf2::tf2
 )
 
+add_library(graph_file_savers SHARED
+    src/plugins/graph_file_savers/geojson_graph_file_saver.cpp
+)
+target_include_directories(graph_file_savers
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(graph_file_savers PUBLIC
+  nav2_core::nav2_core
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav2_msgs_TARGETS}
+  nav2_util::nav2_util_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+  ${geometry_msgs_TARGETS}
+  ${visualization_msgs_TARGETS}
+  ${std_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  tf2_ros::tf2_ros
+  nlohmann_json::nlohmann_json
+)
+target_link_libraries(graph_file_savers PRIVATE
+  ${lifecycle_msgs_TARGETS}
+  rclcpp_components::component
+  tf2::tf2
+)
+
 pluginlib_export_plugin_description_file(nav2_route plugins.xml)
 
 install(DIRECTORY include/
@@ -197,7 +229,8 @@ 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
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -216,7 +249,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include/${PROJECT_NAME})
-ament_export_dependencies(${dependencies})
 ament_export_dependencies(
   rclcpp
   rclcpp_lifecycle
@@ -232,5 +264,6 @@ ament_export_dependencies(
   nanoflann
   nlohmann_json
 )
-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_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_package()

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 <string>
+#include <memory>
+#include <vector>
+#include <unordered_map>
+#include <nlohmann/json.hpp>
+#include <pluginlib/class_loader.hpp>
+
+#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 object 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<tf2_ros::Buffer> 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 = "");
+
+  /**
+   * @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);
+
+protected:
+  std::string route_frame_, graph_filepath_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  rclcpp::Logger logger_{rclcpp::get_logger("GraphSaver")};
+
+  // Graph Parser
+  pluginlib::ClassLoader<GraphFileSaver> 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_

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 <string>
+#include <memory>
+
+
+#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<GraphFileSaver>;
+
+  /**
+   * @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_

nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp

@@ -0,0 +1,99 @@
+// 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 <memory>
+#include <string>
+#include <vector>
+#include <nlohmann/json.hpp>
+
+#include "nav2_core/route_exceptions.hpp"
+#include "nav2_route/interfaces/graph_file_saver.hpp"
+#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.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> & 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> & 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")};
+};
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_

nav2_route/include/nav2_route/types.hpp

@@ -61,6 +61,8 @@ typedef Node * NodePtr;
 typedef std::vector<Node> NodeVector;
 typedef NodeVector Graph;
 typedef std::unordered_map<unsigned int, unsigned int> GraphToIDMap;
+typedef std::unordered_map<unsigned int, std::vector<unsigned int>> GraphToIncomingEdgesMap;
+typedef std::vector<NodePtr> NodePtrVector;
 typedef std::pair<float, NodePtr> NodeElement;
 typedef std::pair<unsigned int, unsigned int> NodeExtents;
 

nav2_route/include/nav2_route/utils.hpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <limits>
 #include <string>
 #include <vector>
 #include "std_msgs/msg/color_rgba.hpp"
@@ -89,6 +90,9 @@
 
   unsigned int marker_idx = 1;
   for (unsigned int i = 0; i != graph.size(); i++) {
+    if (graph[i].nodeid == std::numeric_limits<int>::max()) {
+      continue;  // Skip "deleted" nodes
+    }
     curr_marker.ns = "route_graph";
     curr_marker.id = marker_idx++;
     curr_marker.type = visualization_msgs::msg::Marker::SPHERE;

nav2_route/plugins.xml

@@ -61,6 +61,11 @@
       <description>Parse the geojson graph file into the graph data type</description>
     </class>
   </library>
+  <library path="graph_file_savers">
+    <class type="nav2_route::GeoJsonGraphFileSaver" base_class_type="nav2_route::GraphFileSaver">
+      <description>Save a route graph to a geojson graph file</description>
+    </class>
+  </library>
 
   <library path="route_operations">
     <class type="nav2_route::CollisionMonitor" base_class_type="nav2_route::RouteOperation">