Save routes refactor

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<index>".
+ * @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_

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 <fstream>
-
-#include <yaml-cpp/yaml.h>
-
 #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<std::string> 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();

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 <fstream>
+#include <string>
+#include <vector>
+
 #include <yaml-cpp/yaml.h>
 
 namespace easynav
@@ -143,4 +147,82 @@ 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<std::string> 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;
+
+  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;
+    return false;
+  }
+  file << out.c_str();
+  file.close();
+  if (file.fail()) {
+    error_message = "Failed writing routes to file: " + yaml_file;
+    return false;
+  }
+
+  return true;
+}
+
 }  // namespace easynav

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)
 

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,115 @@ 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
+// (<node>/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<rclcpp_lifecycle::LifecycleNode>(
+    "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<std::string>{"routes_costmap"});
+  node->declare_parameter(
+    "routes.routes_costmap.plugin",
+    std::string("easynav_routes_maps_manager/RoutesCostmapFilter"));
+
+  auto manager = std::make_shared<RoutesMapsManager>();
+  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<easynav_routes_maps_manager::msg::RoutesMap>(
+    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<Costmap2D>("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_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);
+  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<Costmap2D>("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);