From 43d89d1a72e0d3e31e6532d0abaa0cf6dcb71ac8 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 15 Sep 2023 10:47:54 +0900 Subject: [PATCH] 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; }