From 06fe8ec7927669150d3f30e6a16ad9f549b57dc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Aug 2026 09:34:45 +0200 Subject: [PATCH 1/2] Accept incoming routes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 34 +++- .../RoutesMapsManager.hpp | 31 +--- .../easynav_routes_maps_manager/route_io.hpp | 68 ++++++++ .../routes_map.hpp | 59 +++++++ .../msg/RouteSegment.msg | 7 + .../msg/RoutesMap.msg | 8 + .../easynav_routes_maps_manager/package.xml | 5 + .../RoutesMapsManager.cpp | 135 ++-------------- .../easynav_routes_maps_manager/route_io.cpp | 146 ++++++++++++++++++ .../tests/CMakeLists.txt | 4 +- .../tests/routes_mapsmanager_tests.cpp | 70 +++++++++ 11 files changed, 417 insertions(+), 150 deletions(-) create mode 100644 maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/route_io.hpp create mode 100644 maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/routes_map.hpp create mode 100644 maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg create mode 100644 maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg create mode 100644 maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index b7b124ac..d8262de7 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/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 $ $ ) -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() diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp index e7d56c86..14ff362a 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp +++ b/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; - /** * @class RoutesMapsManager * @brief A plugin-based map manager using the RoutesMap data structure. @@ -147,6 +126,12 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @brief Service for saving current routes back to disk. rclcpp::Service::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::SharedPtr + incoming_routes_sub_; }; } // namespace easynav diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/route_io.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/route_io.hpp new file mode 100644 index 00000000..c06b9fd2 --- /dev/null +++ b/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 + +#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_ diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/routes_map.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/routes_map.hpp new file mode 100644 index 00000000..c24b86ff --- /dev/null +++ b/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 +#include + +#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; + +} // namespace easynav + +#endif // EASYNAV_ROUTES_MAPS_MANAGER__ROUTES_MAP_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg b/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg new file mode 100644 index 00000000..634f6b5f --- /dev/null +++ b/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 RoutesMapsManager.hpp), minus its +# UI-editor-only `edit_mode` field. + +string id +geometry_msgs/Pose start +geometry_msgs/Pose end diff --git a/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg b/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg new file mode 100644 index 00000000..61b6f18e --- /dev/null +++ b/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 RoutesMapsManager.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 diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 2f2faa7a..e74b64ca 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -8,6 +8,7 @@ Apache-2.0 ament_cmake + rosidl_default_generators rclcpp rclcpp_lifecycle @@ -25,10 +26,14 @@ yaets yaml-cpp + rosidl_default_runtime + ament_lint_auto ament_lint_common ament_cmake_gtest + rosidl_interface_packages + ament_cmake diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp index d12884bf..c6efe201 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -16,6 +16,7 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include "easynav_common/RTTFBuffer.hpp" +#include "easynav_routes_maps_manager/route_io.hpp" #include @@ -186,6 +187,19 @@ void RoutesMapsManager::on_initialize() throw std::runtime_error(std::string{"Failed to load routes: "} + e.what()); } + // A message received here replaces routes_ outright (last-writer-wins, + // same convention as easynav_costmap_maps_manager's own "incoming_map" + // topic) -- e.g. a fleet-wide navigation manager publishing on + // /global_routes, remapped to this topic. + incoming_routes_sub_ = node->create_subscription( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_routes", + rclcpp::QoS(1).transient_local().reliable(), + [this](easynav_routes_maps_manager::msg::RoutesMap::UniquePtr msg) { + routes_ = from_msg(*msg); + publish_routes_markers(); + publish_interactive_markers(); + }); + // Instantiate and initialize configured route filters for (const auto & filter_name : routes_filters_names) { std::string plugin; @@ -243,126 +257,7 @@ void RoutesMapsManager::update(NavState & nav_state) void RoutesMapsManager::load_routes_from_yaml() { - routes_.clear(); - - if (map_path_.empty()) { - // No map path configured: initialize a default single route segment. - RouteSegment segment; - segment.id = "route0"; - segment.start.position.x = 0.0; - segment.start.position.y = 0.0; - segment.start.position.z = 0.0; - segment.start.orientation.x = 0.0; - segment.start.orientation.y = 0.0; - segment.start.orientation.z = 0.0; - segment.start.orientation.w = 1.0; - - segment.end.position.x = 1.0; - segment.end.position.y = 0.0; - segment.end.position.z = 0.0; - segment.end.orientation.x = 0.0; - segment.end.orientation.y = 0.0; - segment.end.orientation.z = 0.0; - segment.end.orientation.w = 1.0; - - routes_.push_back(segment); - next_route_id_ = 1; - return; - } - - YAML::Node root; - try { - root = YAML::LoadFile(map_path_); - } catch (const std::exception &) { - // File missing or invalid: fall back to a default single route. - RouteSegment segment; - segment.id = "route0"; - segment.start.position.x = 0.0; - segment.start.position.y = 0.0; - segment.start.position.z = 0.0; - segment.start.orientation.x = 0.0; - segment.start.orientation.y = 0.0; - segment.start.orientation.z = 0.0; - segment.start.orientation.w = 1.0; - - segment.end.position.x = 1.0; - segment.end.position.y = 0.0; - segment.end.position.z = 0.0; - segment.end.orientation.x = 0.0; - segment.end.orientation.y = 0.0; - segment.end.orientation.z = 0.0; - segment.end.orientation.w = 1.0; - - routes_.push_back(segment); - next_route_id_ = 1; - return; - } - - if (!root["routes"]) { - // No explicit routes list: use a default single route. - RouteSegment segment; - segment.id = "route0"; - segment.start.position.x = 0.0; - segment.start.position.y = 0.0; - segment.start.position.z = 0.0; - segment.start.orientation.x = 0.0; - segment.start.orientation.y = 0.0; - segment.start.orientation.z = 0.0; - segment.start.orientation.w = 1.0; - - segment.end.position.x = 1.0; - segment.end.position.y = 0.0; - segment.end.position.z = 0.0; - segment.end.orientation.x = 0.0; - segment.end.orientation.y = 0.0; - segment.end.orientation.z = 0.0; - segment.end.orientation.w = 1.0; - - routes_.push_back(segment); - next_route_id_ = 1; - return; - } - - // routes: [route1, route2, ...] - const auto & names_node = root["routes"]; - for (std::size_t i = 0; i < names_node.size(); ++i) { - const auto name = names_node[i].as(); - - if (!root[name]) { - continue; - } - - const auto & route_node = root[name]; - if (!route_node["start"] || !route_node["end"]) { - continue; - } - - RouteSegment segment; - segment.id = name; - - const auto & start = route_node["start"]; - const auto & end = route_node["end"]; - - segment.start.position.x = start["x"].as(); - segment.start.position.y = start["y"].as(); - segment.start.position.z = start["z"].as(0.0); - - segment.start.orientation.x = start["qx"].as(0.0); - segment.start.orientation.y = start["qy"].as(0.0); - segment.start.orientation.z = start["qz"].as(0.0); - segment.start.orientation.w = start["qw"].as(1.0); - - segment.end.position.x = end["x"].as(); - segment.end.position.y = end["y"].as(); - segment.end.position.z = end["z"].as(0.0); - - segment.end.orientation.x = end["qx"].as(0.0); - segment.end.orientation.y = end["qy"].as(0.0); - segment.end.orientation.z = end["qz"].as(0.0); - segment.end.orientation.w = end["qw"].as(1.0); - - routes_.push_back(segment); - } + routes_ = easynav::load_routes_from_yaml(map_path_); // Initialize next_route_id_ so that newly created routes get // unique IDs that don't clash with existing ones. diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp new file mode 100644 index 00000000..408da003 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp @@ -0,0 +1,146 @@ +// 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. + +#include "easynav_routes_maps_manager/route_io.hpp" + +#include + +namespace easynav +{ + +namespace +{ + +RouteSegment default_segment() +{ + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + return segment; +} + +} // namespace + +RoutesMap load_routes_from_yaml(const std::string & yaml_file) +{ + RoutesMap routes; + + if (yaml_file.empty()) { + routes.push_back(default_segment()); + return routes; + } + + YAML::Node root; + try { + root = YAML::LoadFile(yaml_file); + } catch (const std::exception &) { + // File missing or invalid: fall back to a default single route. + routes.push_back(default_segment()); + return routes; + } + + if (!root["routes"]) { + // No explicit routes list: use a default single route. + routes.push_back(default_segment()); + return routes; + } + + // routes: [route1, route2, ...] + const auto & names_node = root["routes"]; + for (std::size_t i = 0; i < names_node.size(); ++i) { + const auto name = names_node[i].as(); + + if (!root[name]) { + continue; + } + + const auto & route_node = root[name]; + if (!route_node["start"] || !route_node["end"]) { + continue; + } + + RouteSegment segment; + segment.id = name; + + const auto & start = route_node["start"]; + const auto & end = route_node["end"]; + + segment.start.position.x = start["x"].as(); + segment.start.position.y = start["y"].as(); + segment.start.position.z = start["z"].as(0.0); + + segment.start.orientation.x = start["qx"].as(0.0); + segment.start.orientation.y = start["qy"].as(0.0); + segment.start.orientation.z = start["qz"].as(0.0); + segment.start.orientation.w = start["qw"].as(1.0); + + segment.end.position.x = end["x"].as(); + segment.end.position.y = end["y"].as(); + segment.end.position.z = end["z"].as(0.0); + + segment.end.orientation.x = end["qx"].as(0.0); + segment.end.orientation.y = end["qy"].as(0.0); + segment.end.orientation.z = end["qz"].as(0.0); + segment.end.orientation.w = end["qw"].as(1.0); + + routes.push_back(segment); + } + + return routes; +} + +easynav_routes_maps_manager::msg::RoutesMap to_msg(const RoutesMap & routes) +{ + easynav_routes_maps_manager::msg::RoutesMap msg; + msg.routes.reserve(routes.size()); + for (const auto & seg : routes) { + easynav_routes_maps_manager::msg::RouteSegment seg_msg; + seg_msg.id = seg.id; + seg_msg.start = seg.start; + seg_msg.end = seg.end; + msg.routes.push_back(seg_msg); + } + return msg; +} + +RoutesMap from_msg(const easynav_routes_maps_manager::msg::RoutesMap & msg) +{ + RoutesMap routes; + routes.reserve(msg.routes.size()); + for (const auto & seg_msg : msg.routes) { + RouteSegment seg; + seg.id = seg_msg.id; + seg.start = seg_msg.start; + seg.end = seg_msg.end; + routes.push_back(seg); + } + return routes; +} + +} // namespace easynav diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt index 3638b4d4..24c91a75 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -3,7 +3,7 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(routes_mapsmanager_tests routes_mapsmanager_tests.cpp) target_link_libraries(routes_mapsmanager_tests - ${PROJECT_NAME} + ${PROJECT_NAME}_lib easynav_common::easynav_common rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle) @@ -11,7 +11,7 @@ target_link_libraries(routes_mapsmanager_tests ament_add_gtest(routes_costmap_filter_tests routes_costmap_filter_tests.cpp) target_link_libraries(routes_costmap_filter_tests - ${PROJECT_NAME} + ${PROJECT_NAME}_lib easynav_common::easynav_common easynav_costmap_common::easynav_costmap_common rclcpp::rclcpp diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp index 84cac5ab..376ced4c 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -15,12 +15,15 @@ #include +#include #include +#include #include "easynav_common/types/NavState.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" +#include "easynav_routes_maps_manager/msg/routes_map.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -213,6 +216,73 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); } +TEST_F(RoutesMapsManagerTest, IncomingRoutesTopicUpdatesInternalAndNavState) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_incoming"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + // Before any message arrives, the manager holds the (empty-path) + // default single segment. + ASSERT_EQ(manager->get_routes().size(), 1u); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + + const std::string topic = + node->get_fully_qualified_name() + std::string("/routes/incoming_routes"); + auto pub = node->create_publisher( + topic, rclcpp::QoS(1).transient_local().reliable()); + pub->on_activate(); + + easynav_routes_maps_manager::msg::RoutesMap msg; + + easynav_routes_maps_manager::msg::RouteSegment seg1; + seg1.id = "incoming1"; + seg1.start.position.x = 5.0; + seg1.end.position.x = 6.0; + seg1.end.orientation.w = 1.0; + msg.routes.push_back(seg1); + + easynav_routes_maps_manager::msg::RouteSegment seg2; + seg2.id = "incoming2"; + seg2.start.position.y = 7.0; + seg2.end.position.y = 8.0; + seg2.end.orientation.w = 1.0; + msg.routes.push_back(seg2); + + pub->publish(msg); + + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 2u); + EXPECT_EQ(routes[0].id, "incoming1"); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 5.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 6.0); + EXPECT_EQ(routes[1].id, "incoming2"); + EXPECT_DOUBLE_EQ(routes[1].start.position.y, 7.0); + EXPECT_DOUBLE_EQ(routes[1].end.position.y, 8.0); + + easynav::NavState nav_state; + manager->update(nav_state); + ASSERT_TRUE(nav_state.has("routes")); + const auto & nav_routes = nav_state.get("routes"); + ASSERT_EQ(nav_routes.size(), 2u); + EXPECT_EQ(nav_routes[0].id, "incoming1"); + EXPECT_EQ(nav_routes[1].id, "incoming2"); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 8600a3ab208ae19d278fd541b7ac6384aac59ec8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Aug 2026 09:54:26 +0200 Subject: [PATCH 2/2] Applying feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RoutesMapsManager.hpp | 8 ++++++++ .../msg/RouteSegment.msg | 4 ++-- .../easynav_routes_maps_manager/msg/RoutesMap.msg | 2 +- .../RoutesMapsManager.cpp | 13 +++++++++++-- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp index 14ff362a..576cc09a 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp @@ -94,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(); diff --git a/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg b/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg index 634f6b5f..1d9d4317 100644 --- a/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg +++ b/maps_managers/easynav_routes_maps_manager/msg/RouteSegment.msg @@ -1,6 +1,6 @@ # A single directed segment between two poses -- the wire form of -# easynav::RouteSegment (see RoutesMapsManager.hpp), minus its -# UI-editor-only `edit_mode` field. +# easynav::RouteSegment (see routes_map.hpp), minus its UI-editor-only +# `edit_mode` field. string id geometry_msgs/Pose start diff --git a/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg b/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg index 61b6f18e..b0f27ecd 100644 --- a/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg +++ b/maps_managers/easynav_routes_maps_manager/msg/RoutesMap.msg @@ -1,5 +1,5 @@ # A full set of navigation routes -- the wire form of easynav::RoutesMap -# (see RoutesMapsManager.hpp). Published on /global_routes by +# (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 diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp index c6efe201..890b3e58 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -196,6 +196,7 @@ void RoutesMapsManager::on_initialize() rclcpp::QoS(1).transient_local().reliable(), [this](easynav_routes_maps_manager::msg::RoutesMap::UniquePtr msg) { routes_ = from_msg(*msg); + recompute_next_route_id(); publish_routes_markers(); publish_interactive_markers(); }); @@ -258,9 +259,17 @@ void RoutesMapsManager::update(NavState & nav_state) void RoutesMapsManager::load_routes_from_yaml() { routes_ = easynav::load_routes_from_yaml(map_path_); + recompute_next_route_id(); +} - // Initialize next_route_id_ so that newly created routes get - // unique IDs that don't clash with existing ones. +void RoutesMapsManager::recompute_next_route_id() +{ + // (Re)initialize next_route_id_ from whatever is currently in routes_, + // so that newly created routes (the interactive marker's "add_segment" + // control) get unique IDs that don't clash with existing ones -- + // called both after a fresh YAML load and after an incoming_routes + // message replaces routes_ wholesale, since either can introduce IDs + // (e.g. "route7") this manager itself never generated. next_route_id_ = 0; for (const auto & seg : routes_) { if (seg.id.rfind("route", 0) == 0 && seg.id.size() > 5) {