Restore original style for swept collision checker

Author: SteveMacenski

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -92,6 +92,24 @@ class GridCollisionChecker
     const unsigned int & i,
     const bool & traverse_unknown);
 
+  /**
+   * @brief Check if in collision by sweeping between two poses
+   * @param x0 X coordinate of starting pose
+   * @param y0 Y coordinate of starting pose
+   * @param theta0 Angle bin number of starting pose (NOT radians)
+   * @param x1 X coordinate of ending pose
+   * @param y1 Y coordinate of ending pose
+   * @param theta1 Angle bin number of ending pose (NOT radians)
+   * @param traverse_unknown Whether or not to traverse in unknown space
+   * @param min_turning_radius Minimum turning radius in grid coordinates
+   * @return boolean if in collision or not
+   */
+  bool inCollision(
+    const float & x0, const float & y0, const float & theta0,
+    const float & x1, const float & y1, const float & theta1,
+    const bool & traverse_unknown,
+    const float & min_turning_radius);
+
   /**
    * @brief Get cost at footprint pose in costmap
    * @return the cost at the pose in costmap

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -484,6 +484,7 @@ class NodeHybrid
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   NAV2_SMAC_PLANNER_COMMON_EXPORT static LookupTable dist_heuristic_lookup_table;
   NAV2_SMAC_PLANNER_COMMON_EXPORT static float size_lookup;
+  NAV2_SMAC_PLANNER_COMMON_EXPORT static bool use_swept_collision_checker;
 
 private:
   float _cell_cost;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -420,6 +420,7 @@ class NodeLattice
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   NAV2_SMAC_PLANNER_COMMON_EXPORT static LookupTable dist_heuristic_lookup_table;
   NAV2_SMAC_PLANNER_COMMON_EXPORT static float size_lookup;
+  NAV2_SMAC_PLANNER_COMMON_EXPORT static bool use_swept_collision_checker;
 
 private:
   float _cell_cost;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -120,6 +120,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   double _max_planning_time;
   double _lookup_table_size;
   double _minimum_turning_radius_global_coords;
+  bool _use_swept_collision_checker;
   bool _debug_visualizations;
   std::string _motion_model_for_search;
   MotionModel _motion_model;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -115,6 +115,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   double _max_planning_time;
   double _lookup_table_size;
+  bool _use_swept_collision_checker;
   bool _debug_visualizations;
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     _planned_footprints_publisher;

nav2_smac_planner/src/collision_checker.cpp

@@ -12,8 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
+#include <algorithm>
+#include <cmath>
+#include <unordered_set>
+
+#include "nav2_util/line_iterator.hpp"
 #include "nav2_smac_planner/collision_checker.hpp"
 
+
 namespace nav2_smac_planner
 {
 
@@ -182,6 +188,166 @@ bool GridCollisionChecker::inCollision(
   return center_cost_ >= INSCRIBED_COST;
 }
 
+bool GridCollisionChecker::inCollision(
+  const float & x0, const float & y0, const float & theta0,
+  const float & x1, const float & y1, const float & theta1,
+  const bool & traverse_unknown,
+  const float & min_turning_radius)
+{
+  // Convert angle bins to radians
+  float start_theta = angles_[static_cast<unsigned int>(theta0)];
+  float end_theta = angles_[static_cast<unsigned int>(theta1)];
+  float dtheta = end_theta - start_theta;
+  if (dtheta > M_PI) {
+    dtheta -= 2.0f * static_cast<float>(M_PI);
+  } else if (dtheta < -M_PI) {
+    dtheta += 2.0f * static_cast<float>(M_PI);
+  }
+
+  // Quick checks to see if sweeping is necessary. If both poses are well
+  // clear of obstacles, fall back to the single pose check on the child node
+  const unsigned int size_x = costmap_->getSizeInCellsX();
+  const unsigned int size_y = costmap_->getSizeInCellsY();
+
+  if (outsideRange(size_x, x0) || outsideRange(size_y, y0) ||
+    outsideRange(size_x, x1) || outsideRange(size_y, y1))
+  {
+    return true;
+  }
+
+  unsigned int mx0 = static_cast<unsigned int>(x0 + 0.5f);
+  unsigned int my0 = static_cast<unsigned int>(y0 + 0.5f);
+  unsigned int mx1 = static_cast<unsigned int>(x1 + 0.5f);
+  unsigned int my1 = static_cast<unsigned int>(y1 + 0.5f);
+  float start_cost = static_cast<float>(costmap_->getCost(mx0, my0));
+  float end_cost = static_cast<float>(costmap_->getCost(mx1, my1));
+
+  bool parent_safe = possible_collision_cost_ > 0.0f &&
+    start_cost < possible_collision_cost_;
+  bool child_safe = possible_collision_cost_ > 0.0f &&
+    end_cost < possible_collision_cost_;
+
+  if (parent_safe && child_safe) {
+    return inCollision(x1, y1, theta1, traverse_unknown);
+  }
+
+  float dx = x1 - x0;
+  float dy = y1 - y0;
+  float distance = hypotf(dx, dy);
+  float arc = fabsf(dtheta) * min_turning_radius;
+  int steps = static_cast<int>(ceilf(std::max(std::max(distance, arc), 1.0f)));
+
+  std::unordered_set<unsigned int> cells;
+  float cx = 0.0f, cy = 0.0f;
+  const bool use_arc = fabsf(dtheta) > 1e-3f;
+  if (use_arc) {
+    if (dtheta > 0.0f) {
+      cx = x0 - min_turning_radius * sin(start_theta);
+      cy = y0 + min_turning_radius * cos(start_theta);
+    } else {
+      cx = x0 + min_turning_radius * sin(start_theta);
+      cy = y0 - min_turning_radius * cos(start_theta);
+    }
+  }
+
+  for (int step = 0; step <= steps; ++step) {
+    float t = static_cast<float>(step) / static_cast<float>(steps);
+    float xi, yi, theta;
+    if (use_arc) {
+      theta = start_theta + t * dtheta;
+      if (dtheta > 0.0f) {
+        xi = cx + min_turning_radius * sin(theta);
+        yi = cy - min_turning_radius * cos(theta);
+      } else {
+        xi = cx - min_turning_radius * sin(theta);
+        yi = cy + min_turning_radius * cos(theta);
+      }
+    } else {
+      xi = x0 + t * dx;
+      yi = y0 + t * dy;
+      theta = start_theta;
+    }
+
+    while (theta < 0.0f) {
+      theta += 2.0f * static_cast<float>(M_PI);
+    }
+    while (theta >= 2.0f * static_cast<float>(M_PI)) {
+      theta -= 2.0f * static_cast<float>(M_PI);
+    }
+    unsigned int angle_bin = static_cast<unsigned int>(
+      theta / (2.0f * static_cast<float>(M_PI)) * angles_.size());
+    angle_bin %= angles_.size();
+
+    if (footprint_is_radius_) {
+      if (outsideRange(costmap_->getSizeInCellsX(), xi) ||
+        outsideRange(costmap_->getSizeInCellsY(), yi))
+      {
+        return true;
+      }
+      unsigned int mx = static_cast<unsigned int>(xi + 0.5f);
+      unsigned int my = static_cast<unsigned int>(yi + 0.5f);
+      cells.insert(my * size_x + mx);
+      continue;
+    }
+
+    // Always include center cell
+    if (outsideRange(costmap_->getSizeInCellsX(), xi) ||
+      outsideRange(costmap_->getSizeInCellsY(), yi))
+    {
+      return true;
+    }
+    unsigned int mx = static_cast<unsigned int>(xi + 0.5f);
+    unsigned int my = static_cast<unsigned int>(yi + 0.5f);
+    cells.insert(my * size_x + mx);
+
+    double wx, wy;
+    costmap_->mapToWorld(static_cast<double>(xi), static_cast<double>(yi), wx, wy);
+    const nav2_costmap_2d::Footprint & oriented = oriented_footprints_[angle_bin];
+
+    unsigned int x_prev, y_prev, x_curr, y_curr;
+    if (!worldToMap(wx + oriented[0].x, wy + oriented[0].y, x_prev, y_prev)) {
+      return true;
+    }
+    for (unsigned int i = 1; i < oriented.size(); ++i) {
+      if (!worldToMap(wx + oriented[i].x, wy + oriented[i].y, x_curr, y_curr)) {
+        return true;
+      }
+      for (nav2_util::LineIterator line(x_prev, y_prev, x_curr, y_curr);
+        line.isValid(); line.advance())
+      {
+        cells.insert(line.getY() * size_x + line.getX());
+      }
+      x_prev = x_curr;
+      y_prev = y_curr;
+    }
+    if (!worldToMap(wx + oriented[0].x, wy + oriented[0].y, x_curr, y_curr)) {
+      return true;
+    }
+    for (nav2_util::LineIterator line(x_prev, y_prev, x_curr, y_curr);
+      line.isValid(); line.advance())
+    {
+      cells.insert(line.getY() * size_x + line.getX());
+    }
+  }
+
+  float max_cost = 0.0f;
+  for (auto index : cells) {
+    center_cost_ = static_cast<float>(costmap_->getCost(index));
+    if (center_cost_ == UNKNOWN_COST && traverse_unknown) {
+      continue;
+    }
+    if (center_cost_ >= INSCRIBED_COST) {
+      return true;
+    }
+    if (center_cost_ > max_cost) {
+      max_cost = center_cost_;
+    }
+  }
+
+  center_cost_ = max_cost;
+  return false;
+}
+
 float GridCollisionChecker::getCost()
 {
   // Assumes inCollision called prior

nav2_smac_planner/src/node_hybrid.cpp

@@ -43,6 +43,7 @@ LookupTable NodeHybrid::dist_heuristic_lookup_table;
 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> NodeHybrid::costmap_ros = nullptr;
 
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
+bool NodeHybrid::use_swept_collision_checker = false;
 
 // Each of these tables are the projected motion models through
 // time and space applied to the search on the current node in
@@ -382,8 +383,15 @@ bool NodeHybrid::isNodeValid(
     return _is_node_valid;
   }
 
-  _is_node_valid = !collision_checker->inCollision(
-    this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown);
+  if (use_swept_collision_checker && parent) {
+    _is_node_valid = !collision_checker->inCollision(
+      parent->pose.x, parent->pose.y, parent->pose.theta,
+      this->pose.x, this->pose.y, this->pose.theta,
+      traverse_unknown, motion_table.min_turning_radius);
+  } else {
+    _is_node_valid = !collision_checker->inCollision(
+      this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown);
+  }
   _cell_cost = collision_checker->getCost();
   return _is_node_valid;
 }

nav2_smac_planner/src/node_lattice.cpp

@@ -39,6 +39,7 @@ namespace nav2_smac_planner
 LatticeMotionTable NodeLattice::motion_table;
 float NodeLattice::size_lookup = 25;
 LookupTable NodeLattice::dist_heuristic_lookup_table;
+bool NodeLattice::use_swept_collision_checker = false;
 
 // Each of these tables are the projected motion models through
 // time and space applied to the search on the current node in
@@ -229,6 +230,15 @@ bool NodeLattice::isNodeValid(
     return _is_node_valid;
   }
 
+  if (use_swept_collision_checker && parent) {
+    _is_node_valid = !collision_checker->inCollision(
+      parent->pose.x, parent->pose.y, parent->pose.theta,
+      this->pose.x, this->pose.y, this->pose.theta,
+      traverse_unknown, motion_table.min_turning_radius);
+    _cell_cost = collision_checker->getCost();
+    return _is_node_valid;
+  }
+
   // Check primitive end pose
   // Convert grid quantization of primitives to radians, then collision checker quantization
   static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -36,7 +36,8 @@ SmacPlannerHybrid::SmacPlannerHybrid()
   _smoother(nullptr),
   _costmap(nullptr),
   _costmap_ros(nullptr),
-  _costmap_downsampler(nullptr)
+  _costmap_downsampler(nullptr),
+  _use_swept_collision_checker(false)
 {
 }
 
@@ -103,6 +104,10 @@ void SmacPlannerHybrid::configure(
   nav2::declare_parameter_if_not_declared(
     node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4));
   node->get_parameter(name + ".minimum_turning_radius", _minimum_turning_radius_global_coords);
+  nav2::declare_parameter_if_not_declared(
+    node, name + ".use_swept_collision_check", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".use_swept_collision_check", _use_swept_collision_checker);
+  NodeHybrid::use_swept_collision_checker = _use_swept_collision_checker;
   nav2::declare_parameter_if_not_declared(
     node, name + ".allow_primitive_interpolation", rclcpp::ParameterValue(false));
   node->get_parameter(
@@ -703,6 +708,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
       } else if (param_name == _name + ".analytic_expansion_max_cost_override") {
         _search_info.analytic_expansion_max_cost_override = parameter.as_bool();
         reinit_a_star = true;
+      } else if (param_name == _name + ".use_swept_collision_check") {
+        _use_swept_collision_checker = parameter.as_bool();
+        NodeHybrid::use_swept_collision_checker = _use_swept_collision_checker;
       }
     } else if (param_type == ParameterType::PARAMETER_INTEGER) {
       if (param_name == _name + ".downsampling_factor") {

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -17,8 +17,8 @@
 #include <vector>
 #include <algorithm>
 #include <limits>
-
 #include "nav2_smac_planner/smac_planner_lattice.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
 
 // #define BENCHMARK_TESTING
 
@@ -32,7 +32,8 @@ SmacPlannerLattice::SmacPlannerLattice()
 : _a_star(nullptr),
   _collision_checker(nullptr, 1, nullptr),
   _smoother(nullptr),
-  _costmap(nullptr)
+  _costmap(nullptr),
+  _use_swept_collision_checker(false)
 {
 }
 
@@ -82,6 +83,12 @@ void SmacPlannerLattice::configure(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".smooth_path", smooth_path);
 
+  nav2::declare_parameter_if_not_declared(
+    node, name + ".use_swept_collision_check", rclcpp::ParameterValue(false));
+  node->get_parameter(
+    name + ".use_swept_collision_check", _use_swept_collision_checker);
+  NodeLattice::use_swept_collision_checker = _use_swept_collision_checker;
+
   // Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model
   nav2::declare_parameter_if_not_declared(
     node, name + ".lattice_filepath", rclcpp::ParameterValue(
@@ -591,6 +598,9 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
       } else if (param_name == _name + ".cache_obstacle_heuristic") {
         reinit_a_star = true;
         _search_info.cache_obstacle_heuristic = parameter.as_bool();
+      } else if (param_name == _name + ".use_swept_collision_check") {
+        _use_swept_collision_checker = parameter.as_bool();
+        NodeLattice::use_swept_collision_checker = _use_swept_collision_checker;
       } else if (param_name == _name + ".allow_reverse_expansion") {
         reinit_a_star = true;
         _search_info.allow_reverse_expansion = parameter.as_bool();