From 65eab414f3ddeccceb988e89af91e645c48d06d6 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 15 Jan 2025 08:11:22 -0800 Subject: [PATCH] Fix goal pose stamp (#4854) (#4855) Signed-off-by: redvinaa (cherry picked from commit 694a222edf7c765c9691fdd10a8025ed6b4aa8ce) Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com> --- .../include/nav2_mppi_controller/tools/path_handler.hpp | 3 ++- nav2_mppi_controller/src/controller.cpp | 2 +- nav2_mppi_controller/src/path_handler.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 56b54af705d..439d21eaf29 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -92,9 +92,10 @@ class PathHandler /** * @brief Get the global goal pose transformed to the local frame + * @param stamp Time to get the goal pose at * @return Transformed goal pose */ - geometry_msgs::msg::PoseStamped getTransformedGoal(); + geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp); protected: /** diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 5edeb1d6aaa..c9ff6e9e0f0 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -84,7 +84,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif std::lock_guard param_lock(*parameters_handler_->getLock()); - geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().pose; + geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 3248f43d083..9eff2144fc3 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -186,10 +186,12 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) plan.poses.erase(plan.poses.begin(), end); } -geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal() +geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal( + const builtin_interfaces::msg::Time & stamp) { auto goal = global_plan_.poses.back(); - goal.header = global_plan_.header; + goal.header.frame_id = global_plan_.header.frame_id; + goal.header.stamp = stamp; if (goal.header.frame_id.empty()) { throw nav2_core::ControllerTFError("Goal pose has an empty frame_id"); }