Fix service zero tolerance numerical issue

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

mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h

@@ -61,11 +61,11 @@ namespace mbf_costmap_nav
 
 struct SearchConfig
 {
-  double angle_increment{ 5 * M_PI / 180 };  // 5 degrees
-  double angle_tolerance{ M_PI_2 };          // 90 degrees
-  double linear_tolerance{ 1.0 };            // 1 meter
-  bool use_padded_fp{ true };                // Padded footprint by default
-  double safety_dist{ 0.1 };                 // 10 cm
+  double angle_max_step_size{ 5 * M_PI / 180 };  // 5 degrees
+  double angle_tolerance{ M_PI_2 };              // 90 degrees
+  double linear_tolerance{ 1.0 };                // 1 meter
+  bool use_padded_fp{ true };                    // Padded footprint by default
+  double safety_dist{ 0.1 };                     // 10 cm
   geometry_msgs::Pose2D goal;
 };
 

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>

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,24 @@ 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 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 =
+      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 =
+      std::abs(angles::shortest_angular_distance(goal.theta, tf::getYaw(response.pose.pose.orientation)));
+  ROS_DEBUG_STREAM("Solution distance: " << linear_dist << ", angle: " << angular_dist);
+
+  // checking if the solution does not violate the requested distance and angle tolerance
+  if (linear_dist > request.dist_tolerance || angular_dist > request.angle_tolerance)
+  {
+    ROS_ERROR_STREAM("Solution violates requested distance and/or angle tolerance");
+    return false;
+  }
+
   response.pose.header.frame_id = costmap_frame;
   response.pose.header.stamp = ros::Time::now();
   response.state = sol.search_state.state;

mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp

@@ -151,10 +151,15 @@ SearchSolution FreePoseSearch::findValidOrientation(const costmap_2d::Costmap2D&
   SearchSolution outside_or_unknown_sol;
   sol.pose = pose_2d;
 
-  for (double dyaw = 0; dyaw <= config.angle_tolerance; dyaw += config.angle_increment)
+  const double reduced_tol = std::min(config.angle_tolerance - std::numeric_limits<double>::epsilon() * M_PI, M_PI);
+  const int num_steps = 1 + std::max(0, static_cast<int>(std::ceil(reduced_tol / config.angle_max_step_size)));
+  const double increment = reduced_tol / std::max(1, num_steps - 1);
+  for (int i = 0; i < num_steps; ++i)
   {
-    const std::vector<double> thetas = dyaw == 0 ? std::vector<double>{ pose_2d.theta } :
-                                                   std::vector<double>{ pose_2d.theta + dyaw, pose_2d.theta - dyaw };
+    const std::vector<double> thetas =
+        i == 0 ? std::vector<double>{ pose_2d.theta } :
+                 std::vector<double>{ pose_2d.theta + i * increment, pose_2d.theta - i * increment };
+
     for (const auto& theta : thetas)
     {
       sol.pose.theta = theta;
@@ -204,19 +209,19 @@ SearchSolution FreePoseSearch::findValidOrientation(const costmap_2d::Costmap2D&
     ROS_DEBUG_STREAM_COND_NAMED(outside_or_unknown_sol.search_state.state == SearchState::UNKNOWN, LOGNAME.data(),
                                 "Solution is in unknown space for pose x-y-theta ("
                                     << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance "
-                                    << config.angle_tolerance << " and increment " << config.angle_increment);
+                                    << config.angle_tolerance << " and increment " << config.angle_max_step_size);
 
     ROS_DEBUG_STREAM_COND_NAMED(outside_or_unknown_sol.search_state.state == SearchState::OUTSIDE, LOGNAME.data(),
                                 "Solution is partially outside the map for pose x-y-theta ("
                                     << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance "
-                                    << config.angle_tolerance << " and increment " << config.angle_increment);
+                                    << config.angle_tolerance << " and increment " << config.angle_max_step_size);
     return outside_or_unknown_sol;
   }
 
   ROS_DEBUG_STREAM_COND_NAMED(sol.search_state.state == SearchState::LETHAL, LOGNAME.data(),
                               "No valid orientation found for pose x-y-theta ("
                                   << pose_2d.x << ", " << pose_2d.y << ", " << pose_2d.theta << ") with tolerance "
-                                  << config.angle_tolerance << " and increment " << config.angle_increment);
+                                  << config.angle_tolerance << " and increment " << config.angle_max_step_size);
 
   return sol;
 }

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)