Fix service zero tolerance numerical issue (#1)

naturerobots · Sep 13, 2023 · 050da88 · 050da88
1 parent c2e7619
commit 050da88
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 1 deletion.
diff --git a/mbf_costmap_nav/CMakeLists.txt b/mbf_costmap_nav/CMakeLists.txt
@@ -5,6 +5,7 @@ set(CMAKE_CXX_STANDARD 17)
 
 find_package(catkin REQUIRED
   COMPONENTS
+  angles
   costmap_2d
   dynamic_reconfigure
   geometry_msgs
@@ -37,6 +38,7 @@ catkin_package(
   CATKIN_DEPENDS
   actionlib
   actionlib_msgs
+  angles
   costmap_2d
   dynamic_reconfigure
   geometry_msgs

diff --git a/mbf_costmap_nav/package.xml b/mbf_costmap_nav/package.xml
@@ -16,6 +16,7 @@
 
     <depend>actionlib</depend>
     <depend>actionlib_msgs</depend>
+    <depend>angles</depend>
     <depend>costmap_2d</depend>
     <depend>dynamic_reconfigure</depend>
     <depend>geometry_msgs</depend>

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
@@ -48,6 +48,7 @@
 #include <nav_core_wrapper/wrapper_local_planner.h>
 #include <nav_core_wrapper/wrapper_recovery_behavior.h>
 #include <xmlrpcpp/XmlRpc.h>
+#include <angles/angles.h>
 
 #include "mbf_costmap_nav/footprint_helper.h"
 #include "mbf_costmap_nav/costmap_navigation_server.h"
@@ -885,7 +886,12 @@ bool CostmapNavigationServer::callServiceFindValidPose(mbf_msgs::FindValidPose::
   response.pose.pose.position.x = sol.pose.x;
   response.pose.pose.position.y = sol.pose.y;
   response.pose.pose.position.z = 0;
-  response.pose.pose.orientation = tf::createQuaternionMsgFromYaw(sol.pose.theta);
+
+  // if the difference between solution angle and requested angle is less than ANGLE_INCREMENT,
+  // use the requested one to avoid numerical issues
+  response.pose.pose.orientation = angles::shortest_angular_distance(goal.theta, sol.pose.theta) < ANGLE_INCREMENT ?
+                                       request.pose.pose.orientation :
+                                       tf::createQuaternionMsgFromYaw(sol.pose.theta);
   response.pose.header.frame_id = costmap_frame;
   response.pose.header.stamp = ros::Time::now();
   response.state = sol.search_state.state;

diff --git a/mbf_costmap_nav/test/free_pose_search_test.cpp b/mbf_costmap_nav/test/free_pose_search_test.cpp
@@ -49,6 +49,7 @@
 #include "mbf_costmap_nav/free_pose_search.h"
 #include "mbf_costmap_nav/costmap_navigation_server.h"
 
+#include <tf2/utils.h>
 namespace mbf_costmap_nav::test
 {
 class SearchHelperTest : public ::testing::Test
@@ -623,6 +624,32 @@ TEST_F(SearchHelperTest, goal_not_centered_small_tolerance)
   auto sol = sh.search();
   EXPECT_EQ(sol.search_state.state, SearchState::LETHAL);
 }
+
+TEST_F(SearchHelperTest, service_zero_tolerance_test)
+{
+  CostmapNavigationServer server(tf_buffer_ptr);
+
+  ros::ServiceClient client = ros::NodeHandle("~").serviceClient<mbf_msgs::FindValidPose>("find_valid_pose");
+  mbf_msgs::FindValidPose::Request req;
+  mbf_msgs::FindValidPose::Response res;
+
+  req.angle_tolerance = 0;
+  req.dist_tolerance = 0;
+  req.use_padded_fp = false;
+  req.costmap = mbf_msgs::FindValidPose::Request::GLOBAL_COSTMAP;
+  req.pose.header.frame_id = "map";
+  req.pose.header.stamp = ros::Time::now();
+  req.pose.pose.position.x = 1.5345;
+  req.pose.pose.position.y = 4.666;
+  req.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.001);
+
+  ASSERT_TRUE(client.call(req, res));
+  EXPECT_EQ(res.state, mbf_msgs::FindValidPose::Response::FREE);
+  EXPECT_EQ(res.pose.pose.position.x, 1.5345);
+  EXPECT_EQ(res.pose.pose.position.y, 4.666);
+  EXPECT_EQ(tf2::getYaw(res.pose.pose.orientation), tf2::getYaw(req.pose.pose.orientation));
+  server.stop();
+}
 }  // namespace mbf_costmap_nav::test
 
 int main(int argc, char** argv)