From e1d602a61d4b2e32099dbdd2669583f41b8d472b Mon Sep 17 00:00:00 2001 From: Vince Reda <60265874+redvinaa@users.noreply.github.com> Date: Wed, 15 Jan 2025 17:07:14 +0100 Subject: [PATCH] Fix goal pose stamp (#4854) Signed-off-by: redvinaa (cherry picked from commit 694a222edf7c765c9691fdd10a8025ed6b4aa8ce) --- .../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 56b54af705..439d21eaf2 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 5edeb1d6aa..c9ff6e9e0f 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 3248f43d08..9eff2144fc 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"); }