Accept incoming routes

maps_managers/easynav_routes_maps_manager/CMakeLists.txt

@@ -21,17 +21,34 @@ find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(interactive_markers REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
+# RouteSegment/RoutesMap -- the wire form of the RouteSegment/RoutesMap
+# types this same package's RoutesMapsManager already defines in C++
+# (see routes_map.hpp), generated here directly rather than in a
+# separate sibling _interfaces package.
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/RouteSegment.msg"
+  "msg/RoutesMap.msg"
+  DEPENDENCIES geometry_msgs
+)
 
-add_library(${PROJECT_NAME} SHARED
+# A C++ library target in this package can't be named ${PROJECT_NAME}:
+# rosidl_generate_interfaces() above already claims that exact target
+# name for the generated-messages target. Keep the *installed file*
+# named libeasynav_routes_maps_manager.so regardless (via OUTPUT_NAME),
+# since the plugin XML files below hardcode that library name.
+add_library(${PROJECT_NAME}_lib SHARED
+  src/easynav_routes_maps_manager/route_io.cpp
   src/easynav_routes_maps_manager/RoutesMapsManager.cpp
   src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp
 )
-target_include_directories(${PROJECT_NAME} PUBLIC
+set_target_properties(${PROJECT_NAME}_lib PROPERTIES OUTPUT_NAME ${PROJECT_NAME})
+target_include_directories(${PROJECT_NAME}_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
-target_link_libraries(${PROJECT_NAME} PUBLIC
+target_link_libraries(${PROJECT_NAME}_lib PUBLIC
   easynav_common::easynav_common
   easynav_core::easynav_core
   easynav_costmap_common::easynav_costmap_common
@@ -49,13 +66,19 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${std_srvs_TARGETS}
 )
 
+# This package both generates (above) and consumes (RoutesMapsManager's
+# own "incoming_routes" subscription) its own RoutesMap message -- link
+# the library against the message typesupport generated for it.
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
+target_link_libraries(${PROJECT_NAME}_lib PUBLIC "${cpp_typesupport_target}")
+
 install(
   DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
 
 install(TARGETS
-  ${PROJECT_NAME}
+  ${PROJECT_NAME}_lib
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -72,7 +95,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")
-ament_export_libraries(${PROJECT_NAME})
+ament_export_libraries(${PROJECT_NAME}_lib)
 ament_export_targets(export_${PROJECT_NAME})
 
 # Register the plugins
@@ -95,5 +118,6 @@ ament_export_dependencies(
   interactive_markers
   yaets
   yaml-cpp
+  rosidl_default_runtime
 )
 ament_package()

maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp

@@ -36,33 +36,12 @@
 #include "easynav_core/MapsManagerBase.hpp"
 
 #include "easynav_routes_maps_manager/RoutesFilter.hpp"
+#include "easynav_routes_maps_manager/routes_map.hpp"
+#include "easynav_routes_maps_manager/msg/routes_map.hpp"
 
 namespace easynav
 {
 
-/// @brief Simple directed segment between two poses.
-///
-/// Each RouteSegment represents a straight-line connection between two
-/// poses in the navigation frame. The segment can be individually
-/// edited and identified via its @ref id field.
-struct RouteSegment
-{
-  /// @brief Unique identifier for this segment.
-  std::string id;
-
-  /// @brief Start pose of the segment.
-  geometry_msgs::msg::Pose start;
-
-  /// @brief End pose of the segment.
-  geometry_msgs::msg::Pose end;
-
-  /// @brief Whether this segment is currently in edit mode.
-  bool edit_mode{false};
-};
-
-/// @brief Container type representing a full set of navigation routes.
-using RoutesMap = std::vector<RouteSegment>;
-
 /**
  * @class RoutesMapsManager
  * @brief A plugin-based map manager using the RoutesMap data structure.
@@ -115,6 +94,14 @@ class RoutesMapsManager : public easynav::MapsManagerBase
   /// from (0, 0, 0) to (1, 0, 0) is created instead.
   void load_routes_from_yaml();
 
+  /// @brief Recompute @ref next_route_id_ from whatever is currently in
+  /// @ref routes_, so newly interactively-created routes get IDs that
+  /// don't clash with existing ones. Called after both a fresh YAML
+  /// load and an incoming_routes message, since either can replace
+  /// @ref routes_ wholesale with IDs this manager itself never
+  /// generated.
+  void recompute_next_route_id();
+
   /// @brief Publish the current routes as visualization markers.
   void publish_routes_markers();
 
@@ -147,6 +134,12 @@ class RoutesMapsManager : public easynav::MapsManagerBase
 
   /// @brief Service for saving current routes back to disk.
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr save_routes_srv_;
+
+  /// @brief Subscription that, on receipt, replaces @ref routes_ with
+  /// whatever was published -- same convention as
+  /// easynav_costmap_maps_manager's own "incoming_map" topic.
+  rclcpp::Subscription<easynav_routes_maps_manager::msg::RoutesMap>::SharedPtr
+    incoming_routes_sub_;
 };
 
 }  // namespace easynav

maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/route_io.hpp

@@ -0,0 +1,68 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// 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.
+
+/// \file
+/// \brief Routes YAML input, and RoutesMap <-> RoutesMap.msg conversion.
+///
+/// Free functions, not RoutesMapsManager members, so that any other
+/// node (e.g. a fleet-wide navigation manager publishing on
+/// /global_routes) can load/convert routes the exact same way
+/// RoutesMapsManager itself does, without depending on
+/// RoutesMapsManager's own heavier machinery (pluginlib,
+/// interactive_markers, ...).
+
+#ifndef EASYNAV_ROUTES_MAPS_MANAGER__ROUTE_IO_HPP_
+#define EASYNAV_ROUTES_MAPS_MANAGER__ROUTE_IO_HPP_
+
+#include <string>
+
+#include "easynav_routes_maps_manager/msg/routes_map.hpp"
+#include "easynav_routes_maps_manager/routes_map.hpp"
+
+namespace easynav
+{
+
+/**
+ * @brief Load a RoutesMap from a routes YAML file.
+ *
+ * Same format used by RoutesMapsManager's own `map_path_file`
+ * parameter: a top-level `routes: [name, ...]` list plus one
+ * `start`/`end` pose-pair entry per name. Falls back to a single
+ * default segment ((0,0,0) -> (1,0,0), id "route0") when the file is
+ * empty, missing, invalid, or has no "routes" key.
+ * @param yaml_file Path to the routes YAML file (empty means "use the
+ *   default segment").
+ * @return The parsed (or default) RoutesMap.
+ */
+RoutesMap load_routes_from_yaml(const std::string & yaml_file);
+
+/**
+ * @brief Convert an in-memory RoutesMap to its wire message form.
+ * @param routes Routes to convert.
+ * @return The equivalent RoutesMap message (edit_mode is not carried
+ *   over -- it is UI-editor-only state).
+ */
+easynav_routes_maps_manager::msg::RoutesMap to_msg(const RoutesMap & routes);
+
+/**
+ * @brief Convert a wire RoutesMap message back to the in-memory form.
+ * @param msg Message to convert.
+ * @return The equivalent RoutesMap.
+ */
+RoutesMap from_msg(const easynav_routes_maps_manager::msg::RoutesMap & msg);
+
+}  // namespace easynav
+
+#endif  // EASYNAV_ROUTES_MAPS_MANAGER__ROUTE_IO_HPP_

maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/routes_map.hpp

@@ -0,0 +1,59 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// 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.
+
+/// \file
+/// \brief Declaration of RouteSegment/RoutesMap, split out of
+/// RoutesMapsManager.hpp so that code needing only the plain data types
+/// (e.g. route_io.hpp, or an external publisher) doesn't have to pull in
+/// RoutesMapsManager's own heavier dependencies (pluginlib,
+/// interactive_markers, ...).
+
+#ifndef EASYNAV_ROUTES_MAPS_MANAGER__ROUTES_MAP_HPP_
+#define EASYNAV_ROUTES_MAPS_MANAGER__ROUTES_MAP_HPP_
+
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace easynav
+{
+
+/// @brief Simple directed segment between two poses.
+///
+/// Each RouteSegment represents a straight-line connection between two
+/// poses in the navigation frame. The segment can be individually
+/// edited and identified via its @ref id field.
+struct RouteSegment
+{
+  /// @brief Unique identifier for this segment.
+  std::string id;
+
+  /// @brief Start pose of the segment.
+  geometry_msgs::msg::Pose start;
+
+  /// @brief End pose of the segment.
+  geometry_msgs::msg::Pose end;
+
+  /// @brief Whether this segment is currently in edit mode.
+  bool edit_mode{false};
+};
+
+/// @brief Container type representing a full set of navigation routes.
+using RoutesMap = std::vector<RouteSegment>;
+
+}  // namespace easynav
+
+#endif  // EASYNAV_ROUTES_MAPS_MANAGER__ROUTES_MAP_HPP_

maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg

@@ -0,0 +1,7 @@
+# A single directed segment between two poses -- the wire form of
+# easynav::RouteSegment (see routes_map.hpp), minus its UI-editor-only
+# `edit_mode` field.
+
+string id
+geometry_msgs/Pose start
+geometry_msgs/Pose end

maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg

@@ -0,0 +1,8 @@
+# A full set of navigation routes -- the wire form of easynav::RoutesMap
+# (see routes_map.hpp). Published on /global_routes by
+# easyfleet_navigation_manager, and accepted by RoutesMapsManager's own
+# "incoming_routes" subscription (same convention as
+# easynav_costmap_maps_manager's "incoming_map"): a message received
+# there replaces whatever was loaded from map_path_file at startup.
+
+RouteSegment[] routes

maps_managers/easynav_routes_maps_manager/package.xml

@@ -8,6 +8,7 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
@@ -25,10 +26,14 @@
   <depend>yaets</depend>
   <depend>yaml-cpp</depend>
 
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>