Backport RouteTool RViz plugin to Jazzy (#5663)

nav2_rviz_plugins/CMakeLists.txt

@@ -16,6 +16,7 @@ set(CMAKE_AUTOMOC ON)
 
 find_package(ament_cmake REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nav2_route REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(nav2_msgs REQUIRED)
@@ -40,12 +41,15 @@ set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/goal_common.hpp
   include/nav2_rviz_plugins/goal_tool.hpp
   include/nav2_rviz_plugins/nav2_panel.hpp
+  include/nav2_rviz_plugins/route_tool.hpp
   include/nav2_rviz_plugins/selector.hpp
   include/nav2_rviz_plugins/utils.hpp
   include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
   include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
 )
 
+qt_wrap_ui(route_tool_UIS_H resource/route_tool.ui)
+
 include_directories(
   include
 )
@@ -57,15 +61,18 @@ add_library(${library_name} SHARED
   src/docking_panel.cpp
   src/goal_tool.cpp
   src/nav2_panel.cpp
+  src/route_tool.cpp
   src/selector.cpp
   src/utils.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}
 )
 
 set(dependencies
   geometry_msgs
+  nav2_route
   nav2_util
   nav2_lifecycle_manager
   nav2_msgs
@@ -90,10 +97,12 @@ ament_target_dependencies(${library_name}
 target_include_directories(${library_name} PUBLIC
   ${Qt5Widgets_INCLUDE_DIRS}
   ${OGRE_INCLUDE_DIRS}
+  ${nav2_route_INCLUDE_DIRS}
 )
 
 target_link_libraries(${library_name}
   rviz_common::rviz_common
+  nav2_route::route_server_core
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -119,9 +128,13 @@ install(
 
 install(
   DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons"
+            "${CMAKE_CURRENT_SOURCE_DIR}/resource"
+            "${CMAKE_CURRENT_SOURCE_DIR}/rviz"
+            "${CMAKE_CURRENT_SOURCE_DIR}/launch"
   DESTINATION "share/${PROJECT_NAME}"
 )
 
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
@@ -136,6 +149,7 @@ ament_export_dependencies(
   map_msgs
   nav_msgs
   rclcpp
+  nav2_route
 )
 
 ament_package()

nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp

@@ -0,0 +1,120 @@
+// 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_RVIZ_PLUGINS__ROUTE_TOOL_HPP_
+#define NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_
+
+#include <ui_route_tool.h>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_route/graph_loader.hpp"
+#include "nav2_route/graph_saver.hpp"
+#include "nav2_route/types.hpp"
+#include "nav2_route/utils.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rviz_common/panel.hpp"
+#include "std_msgs/msg/int16.hpp"
+#include "std_msgs/msg/string.hpp"
+
+
+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 let's the default constructor work and also let's
+           *  someone using the class for something else to pass in a parent
+           *  widget as they normally would with Qt.
+           */
+  explicit 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::route_tool> ui_;
+
+private:
+  void update_route_graph(void);
+  nav2_util::LifecycleNode::SharedPtr node_;
+  std::shared_ptr<nav2_route::GraphLoader> graph_loader_;
+  std::shared_ptr<nav2_route::GraphSaver> graph_saver_;
+  std::shared_ptr<tf2_ros::Buffer> 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<visualization_msgs::msg::MarkerArray>::SharedPtr
+    graph_vis_publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr
+    clicked_point_subscription_;
+
+  unsigned int next_node_id_ = 0;
+};
+}  // namespace nav2_rviz_plugins
+#endif  // NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_

nav2_rviz_plugins/launch/route_tool.launch.py

@@ -0,0 +1,75 @@
+# Copyright (c) 2025 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+import launch_ros.actions
+
+
+def generate_launch_description() -> LaunchDescription:
+
+    # 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,
+        ]
+    )

nav2_rviz_plugins/package.xml

@@ -28,6 +28,7 @@
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>nav2_route</depend>
 
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>

nav2_rviz_plugins/plugins_description.xml

@@ -35,5 +35,11 @@
          base_class_type="rviz_common::Display">
     <description>The Particle Cloud rviz display.</description>
   </class>
+
+  <class name="nav2_rviz_plugins/RouteTool"
+         type="nav2_rviz_plugins::RouteTool"
+         base_class_type="rviz_common::Panel">
+    <description>A tool used to create route graphs.</description>
+  </class>
 
 </library>