solution sanity check

mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp

@@ -888,10 +888,24 @@ bool CostmapNavigationServer::callServiceFindValidPose(mbf_msgs::FindValidPose::
   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 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);
+  // use the requested 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);
+
+  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;