Adjust output time in costamp stack

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp

@@ -182,7 +182,7 @@ class SerestController : public ControllerMethodBase
    * @return double Minimum obstacle distance (m). Infinity if none is found.
    */
   double closest_obstacle_distance(
-    const NavState & nav_state) const;
+    const NavState & nav_state);
 
   /**
    * @brief Compute a safe linear speed bound from obstacle distance and slope.
@@ -226,7 +226,7 @@ class SerestController : public ControllerMethodBase
   void safety_limits(
     const NavState & nav_state,
     const RefKinematics & rk,
-    double & d_closest, double & v_safe, double & v_curv) const;
+    double & d_closest, double & v_safe, double & v_curv);
 
   // Aplica corner‑guard: ajusta v_prog_ref y obtiene omega_boost + término ápice.
   void apply_corner_guard(
@@ -354,6 +354,8 @@ class SerestController : public ControllerMethodBase
   double last_vrot_{0.0};
   /// @brief Last update timestamp.
   rclcpp::Time last_update_ts_;
+  /// @brief Last input timestamp (max of path and odom).
+  rclcpp::Time last_input_ts_;
   /// @brief Output TwistStamped buffer.
   geometry_msgs::msg::TwistStamped twist_stamped_;
 };

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -287,7 +287,7 @@ SerestController::frenet_errors(
 
 double
 SerestController::closest_obstacle_distance(
-  const NavState & nav_state) const
+  const NavState & nav_state)
 {
   // 1) Prefer direct measurement if it exists
   if (nav_state.has("closest_obstacle_distance")) {
@@ -304,14 +304,18 @@ SerestController::closest_obstacle_distance(
   const auto & perceptions = nav_state.get<PointPerceptions>("points");
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  auto fused = PointPerceptionsOpsView(perceptions)
-    .downsample(0.3)
-    .fuse(tf_info.robot_footprint_frame)
-    .filter({-dist_search_radius_, -dist_search_radius_, NAN},
-      {dist_search_radius_, dist_search_radius_, 2.0})
-    .collapse({NAN, NAN, 0.1})
-    .downsample(0.3)
-    .as_points();
+  auto view = PointPerceptionsOpsView(perceptions);
+  view.downsample(0.3)
+  .fuse(tf_info.robot_footprint_frame)
+  .filter({-dist_search_radius_, -dist_search_radius_, NAN},
+    {dist_search_radius_, dist_search_radius_, 2.0})
+  .collapse({NAN, NAN, 0.1})
+  .downsample(0.3);
+  const auto & fused = view.as_points();
+
+  if (last_input_ts_ < view.get_latest_stamp()) {
+    last_input_ts_ = view.get_latest_stamp();
+  }
 
   double min_dist = std::numeric_limits<double>::infinity();
   for (const auto p : fused) {
@@ -377,6 +381,13 @@ SerestController::fetch_required_inputs(
   path = nav_state.get<nav_msgs::msg::Path>("path");
   odom = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
 
+  if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
+    last_input_ts_ = path.header.stamp;
+  }
+  if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
+    last_input_ts_ = odom.header.stamp;
+  }
+
   if (path.poses.empty()) {
     publish_stop(nav_state, path.header.frame_id);
     return false;
@@ -427,7 +438,7 @@ void
 SerestController::safety_limits(
   const NavState & nav_state,
   const RefKinematics & rk,
-  double & d_closest, double & v_safe, double & v_curv) const
+  double & d_closest, double & v_safe, double & v_curv)
 {
   d_closest = closest_obstacle_distance(nav_state);
   v_safe = v_safe_from_distance(d_closest, /*slope_sin=*/0.0);
@@ -535,7 +546,7 @@ SerestController::maybe_final_align_and_publish(
 
   // Publicación y actualización de estado
   twist_stamped_.header.frame_id = path.header.frame_id;
-  twist_stamped_.header.stamp = get_node()->now();
+  twist_stamped_.header.stamp = last_input_ts_;
   twist_stamped_.twist.linear.x = vlin;
   twist_stamped_.twist.angular.z = vrot;
 
@@ -578,7 +589,7 @@ SerestController::publish_cmd_and_debug(
   int in_final_align, int arrived)
 {
   twist_stamped_.header.frame_id = path.header.frame_id;
-  twist_stamped_.header.stamp = get_node()->now();
+  twist_stamped_.header.stamp = last_input_ts_;
   twist_stamped_.twist.linear.x = vlin;
   twist_stamped_.twist.angular.z = vrot;
   nav_state.set("cmd_vel", twist_stamped_);
@@ -601,7 +612,7 @@ SerestController::publish_cmd_and_debug(
 void
 SerestController::publish_stop(NavState & nav_state, const std::string & frame_id)
 {
-  auto now = get_node()->now();
+  auto now = last_input_ts_;
   twist_stamped_.header.frame_id = frame_id;
   twist_stamped_.header.stamp = now;
   twist_stamped_.twist.linear.x = 0.0;
@@ -655,13 +666,12 @@ SerestController::update_rt(NavState & nav_state)
 
   // 6.5) Early turn-in-place with hysteresis (start-of-path aware)
   {
-    const double PI = 3.14159265358979323846;
     const double s_total = pd.s_acc.back();
     const double dist_to_end = s_total - prj.s_star;
 
     // Internal angular thresholds (enter/exit)
-    const double thr_enter = 60.0 * PI / 180.0;
-    const double thr_exit = 35.0 * PI / 180.0;
+    const double thr_enter = 60.0 * M_PI / 180.0;
+    const double thr_exit = 35.0 * M_PI / 180.0;
     const double near_start_s = 0.30;  // treat the first 30 cm as the start region
 
     // Base request from the regular criterion (using the provided threshold)

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -583,19 +583,22 @@ AMCLLocalizer::correct(NavState & nav_state)
   const auto & map_static = nav_state.get<Costmap2D>("map.static");
 
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
-  const auto & filtered = PointPerceptionsOpsView(perceptions)
-    .downsample(map_static.getResolution())
-    .fuse(tf_info.robot_footprint_frame)
-    .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
-    .collapse({NAN, NAN, 0.1})
-    .downsample(map_static.getResolution())
-    .as_points();
+
+  auto view = PointPerceptionsOpsView(perceptions);
+  view.downsample(map_static.getResolution())
+  .fuse(tf_info.robot_footprint_frame)
+  .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
+  .collapse({NAN, NAN, 0.1})
+  .downsample(map_static.getResolution());
+  const auto & filtered = view.as_points();
 
   if (filtered.empty()) {
     RCLCPP_WARN(get_node()->get_logger(), "No points to correct");
     return;
   }
 
+  last_input_time_ = view.get_latest_stamp();
+
   for (auto & particle : particles_) {
     int hits = 0;
     int possible_hits = 0;

maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp

@@ -195,6 +195,10 @@ CostmapMapsManager::update(NavState & nav_state)
 
   nav_state.set("map.dynamic.filtered", dynamic_map_);
 
+  if (!nav_state.has("map_time")) {
+    nav_state.set("map_time", get_node()->now());
+  }
+
   for (const auto & filter : costmap_filters_) {
     filter->update(nav_state);
   }
@@ -203,9 +207,11 @@ CostmapMapsManager::update(NavState & nav_state)
 
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
+  rclcpp::Time map_stamp = nav_state.get<rclcpp::Time>("map_time");
+
   dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_);
   dynamic_grid_msg_.header.frame_id = tf_info.map_frame;
-  dynamic_grid_msg_.header.stamp = get_node()->now();
+  dynamic_grid_msg_.header.stamp = map_stamp;
   dynamic_occ_pub_->publish(dynamic_grid_msg_);
 }
 

maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp

@@ -55,11 +55,15 @@ ObstacleFilter::update(NavState & nav_state)
   Costmap2D & dynamic_map = *dynamic_map_ptr;
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  auto fused = PointPerceptionsOpsView(perceptions)
-    .downsample(dynamic_map.getResolution())
-    .fuse(tf_info.map_frame)
-    .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
-    .as_points();
+  auto view = PointPerceptionsOpsView(perceptions);
+  view.downsample(dynamic_map.getResolution())
+  .fuse(tf_info.map_frame)
+  .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN});
+
+  const auto stamp = view.get_latest_stamp();
+  const auto & fused = view.as_points();
+
+  nav_state.set("map_time", stamp);
 
   double min_x = std::numeric_limits<double>::max();
   double min_y = std::numeric_limits<double>::max();

planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp

@@ -158,6 +158,17 @@ void CostmapPlanner::update(NavState & nav_state)
   const auto & goal = goals.goals.front().pose;
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
+  rclcpp::Time latest_stamp = nav_state.get<rclcpp::Time>("map_time");
+  if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) {
+    latest_stamp = robot_pose.header.stamp;
+  }
+  if (rclcpp::Time(goals.goals.front().header.stamp,
+      latest_stamp.get_clock_type()) > latest_stamp)
+  {
+    latest_stamp = goals.goals.front().header.stamp;
+  }
+  current_path_.header.stamp = latest_stamp;
+
   if (goals.header.frame_id != tf_info.map_frame) {
     RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s",
         goals.header.frame_id.c_str());