From 050da883acfa51c51a1f3badb84b6af2705d24a5 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Wed, 13 Sep 2023 13:47:49 +0900 Subject: [PATCH 1/4] Fix service zero tolerance numerical issue (#1) --- mbf_costmap_nav/CMakeLists.txt | 2 ++ mbf_costmap_nav/package.xml | 1 + .../costmap_navigation_server.cpp | 8 +++++- .../test/free_pose_search_test.cpp | 27 +++++++++++++++++++ 4 files changed, 37 insertions(+), 1 deletion(-) 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/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..cc7fd819 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,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 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) From 242e494b195741df1b4448e549f24990445fb14d Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Thu, 14 Sep 2023 08:58:35 +0900 Subject: [PATCH 2/4] solution sanity check --- .../costmap_navigation_server.cpp | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 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 cc7fd819..292dfbaf 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 @@ -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; From 43d89d1a72e0d3e31e6532d0abaa0cf6dcb71ac8 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 15 Sep 2023 10:47:54 +0900 Subject: [PATCH 3/4] change to angle_max_step_size --- .../include/mbf_costmap_nav/free_pose_search.h | 10 +++++----- .../src/mbf_costmap_nav/free_pose_search.cpp | 17 +++++++++++------ 2 files changed, 16 insertions(+), 11 deletions(-) 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/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; } From e71d2d6e4cc7ec94ade13e730969e8987df90802 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Mon, 18 Sep 2023 10:10:18 +0900 Subject: [PATCH 4/4] 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 =