some fixes for collision detection and performance

ros-navigation · Nov 16, 2023 · 4434898 · 4434898
1 parent ee9b38b
commit 4434898
Show file tree

Hide file tree

Showing 8 changed files with 26 additions and 26 deletions.
diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
@@ -73,7 +73,7 @@ class AnalyticExpansion
    * @brief Sets the collision checker and costmap to use in expansion validation
    * @param collision_checker Collision checker to use
    */
-  void setCollisionChecker(GridCollisionChecker * collision_checker);
+  void setCollisionChecker(GridCollisionChecker * & collision_checker);
 
   /**
    * @brief Attempt an analytic path completion

diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
@@ -116,10 +116,10 @@ class GridCollisionChecker
 protected:
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
-  double footprint_cost_;
+  float footprint_cost_;
   bool footprint_is_radius_;
   std::vector<float> angles_;
-  double possible_inscribed_cost_{-1};
+  float possible_inscribed_cost_{-1};
   rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
   rclcpp::Clock::SharedPtr clock_;
 };

diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
@@ -87,7 +87,7 @@ class Node2D
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -105,7 +105,7 @@ class Node2D
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -123,7 +123,7 @@ class Node2D
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -158,7 +158,7 @@ class Node2D
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }

diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
@@ -217,7 +217,7 @@ class NodeHybrid
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -263,7 +263,7 @@ class NodeHybrid
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -272,7 +272,7 @@ class NodeHybrid
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -289,7 +289,7 @@ class NodeHybrid
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }

diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -184,7 +184,7 @@ class NodeLattice
    * @brief Gets the accumulated cost at this node
    * @return accumulated cost
    */
-  inline float & getAccumulatedCost()
+  inline float getAccumulatedCost()
   {
     return _accumulated_cost;
   }
@@ -202,7 +202,7 @@ class NodeLattice
    * @brief Gets the costmap cost at this node
    * @return costmap cost
    */
-  inline float & getCost()
+  inline float getCost()
   {
     return _cell_cost;
   }
@@ -211,7 +211,7 @@ class NodeLattice
    * @brief Gets if cell has been visited in search
    * @param If cell was visited
    */
-  inline bool & wasVisited()
+  inline bool wasVisited()
   {
     return _was_visited;
   }
@@ -228,7 +228,7 @@ class NodeLattice
    * @brief Gets cell index
    * @return Reference to cell index
    */
-  inline unsigned int & getIndex()
+  inline unsigned int getIndex()
   {
     return _index;
   }

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -110,7 +110,7 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
     _y_size = y_size;
     NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
   }
-  _expander->setCollisionChecker(collision_checker);
+  _expander->setCollisionChecker(_collision_checker);
 }
 
 template<typename NodeT>

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -41,7 +41,7 @@ AnalyticExpansion<NodeT>::AnalyticExpansion(
 
 template<typename NodeT>
 void AnalyticExpansion<NodeT>::setCollisionChecker(
-  GridCollisionChecker * collision_checker)
+  GridCollisionChecker * & collision_checker)
 {
   _collision_checker = collision_checker;
 }
@@ -191,8 +191,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   }
 
   // A move of sqrt(2) is guaranteed to be in a new cell
-  static const float sqrt_2 = std::sqrt(2.);
-  unsigned int num_intervals = std::floor(d / sqrt_2);
+  static const float sqrt_2 = std::sqrt(2.0f);
+  unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));
 
   AnalyticExpansionNodes possible_nodes;
   // When "from" and "to" are zero or one cell away,

diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
@@ -49,7 +49,7 @@ void GridCollisionChecker::setFootprint(
   const bool & radius,
   const double & possible_inscribed_cost)
 {
-  possible_inscribed_cost_ = possible_inscribed_cost;
+  possible_inscribed_cost_ = static_cast<float>(possible_inscribed_cost);
   footprint_is_radius_ = radius;
 
   // Use radius, no caching required
@@ -106,8 +106,8 @@ bool GridCollisionChecker::inCollision(
   if (!footprint_is_radius_) {
     // if footprint, then we check for the footprint's points, but first see
     // if the robot is even potentially in an inscribed collision
-    footprint_cost_ = costmap_->getCost(
-      static_cast<unsigned int>(x), static_cast<unsigned int>(y));
+    footprint_cost_ = static_cast<float>(costmap_->getCost(
+      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
     if (footprint_cost_ < possible_inscribed_cost_) {
       if (possible_inscribed_cost_ > 0) {
@@ -146,7 +146,7 @@ bool GridCollisionChecker::inCollision(
       current_footprint.push_back(new_pt);
     }
 
-    footprint_cost_ = footprintCost(current_footprint);
+    footprint_cost_ = static_cast<float>(footprintCost(current_footprint));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;
@@ -156,15 +156,15 @@ bool GridCollisionChecker::inCollision(
     return footprint_cost_ >= OCCUPIED;
   } else {
     // if radius, then we can check the center of the cost assuming inflation is used
-    footprint_cost_ = costmap_->getCost(
-      static_cast<unsigned int>(x), static_cast<unsigned int>(y));
+    footprint_cost_ = static_cast<float>(costmap_->getCost(
+      static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
     if (footprint_cost_ == UNKNOWN && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return static_cast<double>(footprint_cost_) >= INSCRIBED;
+    return footprint_cost_ >= INSCRIBED;
   }
 }