From 77b75cebbd9048199d0100d785c196c3d86d428d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 16 Aug 2026 11:27:45 +0200 Subject: [PATCH 1/2] Save routes refactor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_routes_maps_manager/route_io.hpp | 22 ++++ .../RoutesMapsManager.cpp | 76 +----------- .../easynav_routes_maps_manager/route_io.cpp | 73 ++++++++++++ .../tests/CMakeLists.txt | 1 + .../tests/routes_mapsmanager_tests.cpp | 111 ++++++++++++++++++ 5 files changed, 213 insertions(+), 70 deletions(-) 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 index c06b9fd..d55b8ba 100644 --- 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 @@ -63,6 +63,28 @@ easynav_routes_maps_manager::msg::RoutesMap to_msg(const RoutesMap & routes); */ RoutesMap from_msg(const easynav_routes_maps_manager::msg::RoutesMap & msg); +/** + * @brief Persist a RoutesMap to a routes YAML file, in the exact format + * load_routes_from_yaml() reads back: a top-level `routes: [name, ...]` + * list plus one `start`/`end` pose-pair entry per name. + * + * A free function (rather than a RoutesMapsManager member) for the same + * reason load_routes_from_yaml()/to_msg()/from_msg() are: so any other + * node wanting to persist edited routes to the same file format -- + * RoutesMapsManager's own `save_routes` service included -- can do so + * without depending on RoutesMapsManager itself. + * + * @param yaml_file Path to write to. + * @param routes Routes to persist. Segment ids are used as the YAML + * keys; segments with an empty id are named "route". + * @param error_message Set to a human-readable reason on failure; + * untouched on success. + * @return true on success, false if the file could not be opened for + * writing (see error_message). + */ +bool save_routes_to_yaml( + const std::string & yaml_file, const RoutesMap & routes, std::string & error_message); + } // namespace easynav #endif // EASYNAV_ROUTES_MAPS_MANAGER__ROUTE_IO_HPP_ 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 890b3e5..83af10f 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 @@ -18,10 +18,6 @@ #include "easynav_common/RTTFBuffer.hpp" #include "easynav_routes_maps_manager/route_io.hpp" -#include - -#include - #include "ament_index_cpp/get_package_share_path.hpp" #include "rclcpp/rclcpp.hpp" @@ -105,74 +101,14 @@ void RoutesMapsManager::on_initialize() [this](const std_srvs::srv::Trigger::Request::SharedPtr, std_srvs::srv::Trigger::Response::SharedPtr response) { try { - // Persist current routes_ back to YAML file using the - // structure: - // routes: [route1, route2] - // route1: { start: ..., end: ... } - YAML::Emitter out; - out << YAML::BeginMap; - - // Collect route names from ids (or generate generic ones). - std::vector names; - names.reserve(routes_.size()); - for (std::size_t i = 0; i < routes_.size(); ++i) { - const auto & seg = routes_[i]; - if (!seg.id.empty()) { - names.push_back(seg.id); - } else { - names.push_back("route" + std::to_string(i)); - } - } - - out << YAML::Key << "routes" << YAML::Value << YAML::Flow << YAML::BeginSeq; - for (const auto & n : names) { - out << n; - } - out << YAML::EndSeq; - - // Now define each route as a separate key in the map. - for (std::size_t i = 0; i < routes_.size(); ++i) { - const auto & seg = routes_[i]; - const auto & name = names[i]; - - out << YAML::Key << name << YAML::Value << YAML::BeginMap; - - out << YAML::Key << "start" << YAML::Value << YAML::BeginMap; - out << YAML::Key << "x" << YAML::Value << seg.start.position.x; - out << YAML::Key << "y" << YAML::Value << seg.start.position.y; - out << YAML::Key << "z" << YAML::Value << seg.start.position.z; - out << YAML::Key << "qx" << YAML::Value << seg.start.orientation.x; - out << YAML::Key << "qy" << YAML::Value << seg.start.orientation.y; - out << YAML::Key << "qz" << YAML::Value << seg.start.orientation.z; - out << YAML::Key << "qw" << YAML::Value << seg.start.orientation.w; - out << YAML::EndMap; - - out << YAML::Key << "end" << YAML::Value << YAML::BeginMap; - out << YAML::Key << "x" << YAML::Value << seg.end.position.x; - out << YAML::Key << "y" << YAML::Value << seg.end.position.y; - out << YAML::Key << "z" << YAML::Value << seg.end.position.z; - out << YAML::Key << "qx" << YAML::Value << seg.end.orientation.x; - out << YAML::Key << "qy" << YAML::Value << seg.end.orientation.y; - out << YAML::Key << "qz" << YAML::Value << seg.end.orientation.z; - out << YAML::Key << "qw" << YAML::Value << seg.end.orientation.w; - out << YAML::EndMap; - - out << YAML::EndMap; - } - - out << YAML::EndMap; - - std::ofstream file(map_path_); - if (!file.is_open()) { + std::string error_message; + if (easynav::save_routes_to_yaml(map_path_, routes_, error_message)) { + response->success = true; + response->message = "Routes saved to " + map_path_; + } else { response->success = false; - response->message = "Could not open file for writing: " + map_path_; - return; + response->message = error_message; } - file << out.c_str(); - file.close(); - - response->success = true; - response->message = "Routes saved to " + map_path_; } catch (const std::exception & e) { response->success = false; response->message = e.what(); 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 index 408da00..7d42d28 100644 --- 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 @@ -15,6 +15,10 @@ #include "easynav_routes_maps_manager/route_io.hpp" +#include +#include +#include + #include namespace easynav @@ -143,4 +147,73 @@ RoutesMap from_msg(const easynav_routes_maps_manager::msg::RoutesMap & msg) return routes; } +bool save_routes_to_yaml( + const std::string & yaml_file, const RoutesMap & routes, std::string & error_message) +{ + // routes: [route1, route2] + // route1: { start: ..., end: ... } + YAML::Emitter out; + out << YAML::BeginMap; + + // Collect route names from ids (or generate generic ones). + std::vector names; + names.reserve(routes.size()); + for (std::size_t i = 0; i < routes.size(); ++i) { + const auto & seg = routes[i]; + if (!seg.id.empty()) { + names.push_back(seg.id); + } else { + names.push_back("route" + std::to_string(i)); + } + } + + out << YAML::Key << "routes" << YAML::Value << YAML::Flow << YAML::BeginSeq; + for (const auto & n : names) { + out << n; + } + out << YAML::EndSeq; + + // Now define each route as a separate key in the map. + for (std::size_t i = 0; i < routes.size(); ++i) { + const auto & seg = routes[i]; + const auto & name = names[i]; + + out << YAML::Key << name << YAML::Value << YAML::BeginMap; + + out << YAML::Key << "start" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.start.position.x; + out << YAML::Key << "y" << YAML::Value << seg.start.position.y; + out << YAML::Key << "z" << YAML::Value << seg.start.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.start.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.start.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.start.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.start.orientation.w; + out << YAML::EndMap; + + out << YAML::Key << "end" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.end.position.x; + out << YAML::Key << "y" << YAML::Value << seg.end.position.y; + out << YAML::Key << "z" << YAML::Value << seg.end.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.end.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.end.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.end.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.end.orientation.w; + out << YAML::EndMap; + + out << YAML::EndMap; + } + + out << YAML::EndMap; + + std::ofstream file(yaml_file); + if (!file.is_open()) { + error_message = "Could not open file for writing: " + yaml_file; + return false; + } + file << out.c_str(); + file.close(); + + return true; +} + } // 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 24c91a7..c5abce4 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -5,6 +5,7 @@ ament_add_gtest(routes_mapsmanager_tests target_link_libraries(routes_mapsmanager_tests ${PROJECT_NAME}_lib easynav_common::easynav_common + easynav_costmap_common::easynav_costmap_common rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle) 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 376ced4..d796e14 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 @@ -22,6 +22,8 @@ #include "easynav_common/types/NavState.hpp" #include "easynav_common/RTTFBuffer.hpp" +#include "easynav_costmap_common/costmap_2d.hpp" + #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include "easynav_routes_maps_manager/msg/routes_map.hpp" @@ -31,6 +33,7 @@ using easynav::RoutesMapsManager; using easynav::RoutesMap; using easynav::RouteSegment; +using easynav::Costmap2D; class RoutesMapsManagerTest : public ::testing::Test { @@ -283,6 +286,114 @@ TEST_F(RoutesMapsManagerTest, IncomingRoutesTopicUpdatesInternalAndNavState) EXPECT_EQ(nav_routes[1].id, "incoming2"); } +// Reproduces the exact scenario a user reported live, in a real +// two-robot deployment: editing a route (via easyfleet_navigation_manager's +// interactive markers on /global_routes, remapped here to this node's +// own incoming_routes) is confirmed to update /global_routes itself, but +// the per-robot routes-costmap-filter output +// (/routes/routes_map) did not seem to reflect the edit live -- +// only after a full restart. Drives RoutesMapsManager (with a real +// RoutesCostmapFilter loaded, exactly as the real deployment configures +// it via routes.filters) through two separate "cycles": an +// incoming_routes edit followed by a manager->update() call, twice in a +// row with a *different* route each time -- resetting the "map" NavState +// key to a fresh, all-zero Costmap2D between the two update() calls, +// exactly as CostmapMapsManager::update() itself does every real system +// cycle (`*dynamic_map_ = map_base_;`, unconditionally, before any +// filter runs). If the second update() still reflects the first route +// instead of the second, that reproduces the bug within +// easynav_routes_maps_manager itself, isolated from Gazebo/localization/ +// the rest of the real system. +TEST_F(RoutesMapsManagerTest, LiveIncomingRoutesEditRefreshesCostmapFilterOnNextCycle) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_live_costmap"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + node->declare_parameter("routes.filters", std::vector{"routes_costmap"}); + node->declare_parameter( + "routes.routes_costmap.plugin", + std::string("easynav_routes_maps_manager/RoutesCostmapFilter")); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + 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(); + + auto publish_single_route = [&](double x0, double x1) { + easynav_routes_maps_manager::msg::RoutesMap msg; + easynav_routes_maps_manager::msg::RouteSegment seg; + seg.id = "liveroute"; + seg.start.position.x = x0; + seg.start.orientation.w = 1.0; + seg.end.position.x = x1; + seg.end.orientation.w = 1.0; + msg.routes.push_back(seg); + pub->publish(msg); + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + }; + + // Cycle 1: route occupies cells [2, 7] of a 10x1, resolution-1.0 costmap. + publish_single_route(2.0, 7.0); + ASSERT_EQ(manager->get_routes().size(), 1u); + ASSERT_DOUBLE_EQ(manager->get_routes()[0].start.position.x, 2.0); + + easynav::NavState nav_state; + Costmap2D map1(10, 1, 1.0, 0.0, 0.0); + for (unsigned int x = 0; x < 10; ++x) { + map1.setCost(x, 0, 0); + } + nav_state.set("map", map1); + + manager->update(nav_state); + + { + const auto & map_after = nav_state.get("map"); + for (unsigned int x = 2; x <= 6; ++x) { + EXPECT_EQ(map_after.getCost(x, 0), 0) << "cycle 1, x=" << x; + } + EXPECT_GE(map_after.getCost(0, 0), 50); + EXPECT_GE(map_after.getCost(9, 0), 50); + } + + // Cycle 2: a live edit moves the route to cells [0, 2], then the next + // cycle's map is reset fresh (as CostmapMapsManager does every real + // cycle) before the routes filter runs again. + publish_single_route(0.0, 2.0); + ASSERT_DOUBLE_EQ(manager->get_routes()[0].start.position.x, 0.0); + + Costmap2D map2(10, 1, 1.0, 0.0, 0.0); + for (unsigned int x = 0; x < 10; ++x) { + map2.setCost(x, 0, 0); + } + nav_state.set("map", map2); + + manager->update(nav_state); + + const auto & map_after2 = nav_state.get("map"); + for (unsigned int x = 0; x <= 1; ++x) { + EXPECT_EQ(map_after2.getCost(x, 0), 0) << + "cycle 2, x=" << x << ": should be on the NEW route, not the old one"; + } + for (unsigned int x = 5; x <= 9; ++x) { + EXPECT_GE(map_after2.getCost(x, 0), 50) << + "cycle 2, x=" << x << ": should be off the NEW route"; + } +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 6dc7f4eca1bb630d3b85fdc28ad730895f0fa242 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 16 Aug 2026 12:45:52 +0200 Subject: [PATCH 2/2] Apply feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_routes_maps_manager/route_io.cpp | 9 +++++++++ .../tests/routes_mapsmanager_tests.cpp | 1 + 2 files changed, 10 insertions(+) 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 index 7d42d28..f2074a2 100644 --- 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 @@ -205,6 +205,11 @@ bool save_routes_to_yaml( out << YAML::EndMap; + if (!out.good()) { + error_message = "Failed to serialize routes to YAML: " + out.GetLastError(); + return false; + } + std::ofstream file(yaml_file); if (!file.is_open()) { error_message = "Could not open file for writing: " + yaml_file; @@ -212,6 +217,10 @@ bool save_routes_to_yaml( } file << out.c_str(); file.close(); + if (file.fail()) { + error_message = "Failed writing routes to file: " + yaml_file; + return false; + } return true; } 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 d796e14..5fe5117 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 @@ -373,6 +373,7 @@ TEST_F(RoutesMapsManagerTest, LiveIncomingRoutesEditRefreshesCostmapFilterOnNext // cycle's map is reset fresh (as CostmapMapsManager does every real // cycle) before the routes filter runs again. publish_single_route(0.0, 2.0); + ASSERT_EQ(manager->get_routes().size(), 1u); ASSERT_DOUBLE_EQ(manager->get_routes()[0].start.position.x, 0.0); Costmap2D map2(10, 1, 1.0, 0.0, 0.0);