Merge pull request #44 from fmrico/adjust_processing_times_costmap

Adjust output time in costamp stack

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

@@ -177,7 +177,8 @@ AMCLLocalizer::AMCLLocalizer()
       double roll, pitch, yaw;
       tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
 
-      ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")";
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << " } Odometry with pose: (x: " <<
+        x << ", y: " << y << ", yaw: " << yaw << ")";
       return ret.str();
     });
 }
@@ -269,6 +270,7 @@ AMCLLocalizer::on_initialize()
     "initialpose", 10, std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1));
 
   last_reseed_ = get_node()->now();
+  last_input_time_ = get_node()->now();
 
   get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug);
 }
@@ -583,19 +585,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;

localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp

@@ -357,7 +357,8 @@ AMCLLocalizer::AMCLLocalizer()
       tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
                         odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
       double r, p, y; tf2::Matrix3x3(q).getRPY(r, p, y);
-      ret << "Odometry with pose: (x: " << odom.pose.pose.position.x
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " <<
+        odom.pose.pose.position.x
           << ", y: " << odom.pose.pose.position.y << ", yaw: " << y << ")";
       return ret.str();
     });

localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp

@@ -175,7 +175,8 @@ AMCLLocalizer::AMCLLocalizer()
       double roll, pitch, yaw;
       tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
 
-      ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")";
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " <<
+        x << ", y: " << y << ", yaw: " << yaw << ")";
       return ret.str();
     });
 }

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,16 @@ 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();
+  rclcpp::Time stamp;
+
+  auto view = PointPerceptionsOpsView(perceptions);
+  view.downsample(dynamic_map.getResolution())
+  .fuse(tf_info.map_frame, stamp, false)
+  .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN});
+
+  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();

maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp

@@ -19,17 +19,6 @@ class CostmapMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override

maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp

@@ -22,17 +22,6 @@ class NavMapMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    ::easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override

maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp

@@ -40,18 +40,6 @@ class SimpleMapsManagerTest : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-
-    easynav::NavState::register_printer<easynav::PointPerceptions>(
-      [](const easynav::PointPerceptions & perceptions) {
-        std::ostringstream ret;
-        ret << "PointPerception " << perceptions.size() << " with:\n";
-        for (const auto & perception : perceptions) {
-          ret << "\t[" << static_cast<const void *>(perception.get()) << "] --> "
-              << perception->data.size() << " points in frame [" << perception->frame_id
-              << "] with ts " << perception->stamp.seconds() << "\n";
-        }
-        return ret.str();
-      });
   }
 
   void TearDown() override