diff --git a/mbf_costmap_nav/CMakeLists.txt b/mbf_costmap_nav/CMakeLists.txt
index ea24d26f..1d1fbd9f 100644
--- 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/include/mbf_costmap_nav/free_pose_search.h b/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h
index c823e864..3cc37491 100644
--- a/mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search.h
+++ b/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;
};
diff --git a/mbf_costmap_nav/package.xml b/mbf_costmap_nav/package.xml
index 35689e0e..2b3c65e4 100644
--- a/mbf_costmap_nav/package.xml
+++ b/mbf_costmap_nav/package.xml
@@ -16,6 +16,7 @@
actionlib
actionlib_msgs
+ angles
costmap_2d
dynamic_reconfigure
geometry_msgs
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 29cde0fd..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
@@ -48,6 +48,7 @@
#include
#include
#include
+#include
#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;
diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp
index a6bc72db..d16985bf 100644
--- a/mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search.cpp
+++ b/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::epsilon() * M_PI, M_PI);
+ const int num_steps = 1 + std::max(0, static_cast(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 thetas = dyaw == 0 ? std::vector{ pose_2d.theta } :
- std::vector{ pose_2d.theta + dyaw, pose_2d.theta - dyaw };
+ const std::vector thetas =
+ i == 0 ? std::vector{ pose_2d.theta } :
+ std::vector{ 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;
}
diff --git a/mbf_costmap_nav/test/free_pose_search_test.cpp b/mbf_costmap_nav/test/free_pose_search_test.cpp
index 574f57ba..e797a167 100644
--- 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
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("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)