change to angle_max_step_size

naturerobots · Sep 15, 2023 · 43d89d1 · 43d89d1
1 parent 242e494
commit 43d89d1
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 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
@@ -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
@@ -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;
 }