From e71d2d6e4cc7ec94ade13e730969e8987df90802 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Mon, 18 Sep 2023 10:10:18 +0900 Subject: [PATCH] fix conversion --- .../src/mbf_costmap_nav/costmap_navigation_server.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp index 292dfbaf..2d4bed06 100644 --- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp +++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp @@ -887,12 +887,10 @@ bool CostmapNavigationServer::callServiceFindValidPose(mbf_msgs::FindValidPose:: response.pose.pose.position.y = sol.pose.y; response.pose.pose.position.z = 0; - // if the difference between solution angle and requested angle is less than ANGLE_INCREMENT, - // use the requested one to avoid violating a very small angle_tolerance (e.g. 0) + // if solution angle and requested angle are the same (after conversion), + // use the requested (quaternion) one to avoid violating a very small angle_tolerance (e.g. 0) response.pose.pose.orientation = - std::abs(angles::shortest_angular_distance(goal.theta, sol.pose.theta)) < ANGLE_INCREMENT ? - request.pose.pose.orientation : - tf::createQuaternionMsgFromYaw(sol.pose.theta); + goal.theta == sol.pose.theta ? request.pose.pose.orientation : tf::createQuaternionMsgFromYaw(sol.pose.theta); const double linear_dist = std::hypot(goal.x - sol.pose.x, goal.y - sol.pose.y); const double angular_dist =