diff --git a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp index 2ca3680..0b90834 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/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_; }; diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index a082674..2cf5511 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/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("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::infinity(); for (const auto p : fused) { @@ -377,6 +381,13 @@ SerestController::fetch_required_inputs( path = nav_state.get("path"); odom = nav_state.get("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) diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 022892c..75f51b2 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/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("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; diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index e84fb9b..6bd2391 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/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(); }); diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 791eed6..30a62e7 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/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(); }); } diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index e38c5a1..229846b 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/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("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_); } diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 234803c..1dd019e 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/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::max(); double min_y = std::numeric_limits::max(); diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index cb338cf..0d05f16 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/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( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index e1ede33..9de01f7 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/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( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 87d5e3d..f46ee0f 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/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( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 59d6d76..2f9b356 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -117,7 +117,8 @@ CostmapPlanner::CostmapPlanner() NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); @@ -158,6 +159,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("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()); diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 044ddee..0f1e711 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -56,7 +56,8 @@ AStarPlanner::AStarPlanner() NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index 3315d17..7d116d2 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -79,7 +79,8 @@ SimplePlanner::SimplePlanner() [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length" << + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str();