From 5f1bca990b066a5705ccc2a872751d44e2971d36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20=C3=96rjehag?= Date: Fri, 21 Jan 2022 10:31:55 +0100 Subject: [PATCH] Bugfix tf lookup of goal pose in nav2_controller MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Erik Örjehag --- .../nav2_controller/controller_server.hpp | 2 +- nav2_controller/src/controller_server.cpp | 23 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 25ec02ef498..d2e70d67d32 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -256,7 +256,7 @@ class ControllerServer : public nav2_util::LifecycleNode double failure_tolerance_; // Whether we've published the single controller warning yet - geometry_msgs::msg::Pose end_pose_; + geometry_msgs::msg::PoseStamped end_pose_; // Last time the controller generated a valid command rclcpp::Time last_valid_cmd_time_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1e8cad4dcd2..42771344fc6 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -429,18 +429,14 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = path.poses.back(); - end_pose.header.frame_id = path.header.frame_id; - rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); - nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), - end_pose, end_pose, tolerance); + end_pose_ = path.poses.back(); + end_pose_.header.frame_id = path.header.frame_id; goal_checkers_[current_goal_checker_]->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", - end_pose.pose.position.x, end_pose.pose.position.y); - end_pose_ = end_pose.pose; + end_pose_.pose.position.x, end_pose_.pose.position.y); + current_path_ = path; } @@ -567,7 +563,16 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return goal_checkers_[current_goal_checker_]->isGoalReached(pose.pose, end_pose_, velocity); + + geometry_msgs::msg::PoseStamped transformed_end_pose; + rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); + nav_2d_utils::transformPose( + costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), + end_pose_, transformed_end_pose, tolerance); + + return goal_checkers_[current_goal_checker_]->isGoalReached( + pose.pose, transformed_end_pose.pose, + velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)