Fixes for nav2_smac_planner to build with -Werror.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

nav2_smac_planner/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
+nav2_package()
+
 if(MSVC)
   add_compile_definitions(_USE_MATH_DEFINES)
 else()

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -59,11 +59,11 @@ inline MotionModel fromString(const std::string & n)
   }
 }
 
-const float UNKNOWN = 255.0;
-const float OCCUPIED = 254.0;
-const float INSCRIBED = 253.0;
-const float MAX_NON_OBSTACLE = 252.0;
-const float FREE = 0;
+const float UNKNOWN_COST = 255.0;
+const float OCCUPIED_COST = 254.0;
+const float INSCRIBED_COST = 253.0;
+const float MAX_NON_OBSTACLE_COST = 252.0;
+const float FREE_COST = 0;
 
 }  // namespace nav2_smac_planner
 

nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp

@@ -37,9 +37,9 @@ class NodeBasic
    * @brief A constructor for nav2_smac_planner::NodeBasic
    * @param index The index of this node for self-reference
    */
-  explicit NodeBasic(const uint64_t index)
-  : index(index),
-    graph_node_ptr(nullptr)
+  explicit NodeBasic(const uint64_t new_index)
+  : graph_node_ptr(nullptr),
+    index(new_index)
   {
   }
 

nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h

@@ -147,7 +147,7 @@ static Counts& counts() {
 #        pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD))
 #        define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x)                                       \
             [](size_t mask) noexcept -> int {                                             \
-                unsigned long index;  // NOLINT                                           \
+                unsigned long index;                                                      \
                 return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast<int>(index) \
                                                                 : ROBIN_HOOD(BITNESS);    \
             }(x)

nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -110,7 +110,7 @@ struct SmootherParams
  * @struct nav2_smac_planner::TurnDirection
  * @brief A struct with the motion primitive's direction embedded
  */
-enum struct TurnDirection
+enum class TurnDirection
 {
   UNKNOWN = 0,
   FORWARD = 1,

nav2_smac_planner/src/analytic_expansion.cpp

@@ -342,6 +342,11 @@ getAnalyticPath(
   const NodeGetter & node_getter,
   const ompl::base::StateSpacePtr & state_space)
 {
+  (void)node;
+  (void)goal;
+  (void)node_getter;
+  (void)state_space;
+
   return AnalyticExpansionNodes();
 }
 
@@ -351,6 +356,10 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
   const NodePtr & goal_node,
   const AnalyticExpansionNodes & expanded_nodes)
 {
+  (void)node;
+  (void)goal_node;
+  (void)expanded_nodes;
+
   return NodePtr(nullptr);
 }
 
@@ -360,6 +369,12 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyt
   const NodeGetter & getter, int & analytic_iterations,
   int & closest_distance)
 {
+  (void)current_node;
+  (void)goal_node;
+  (void)getter;
+  (void)analytic_iterations;
+  (void)closest_distance;
+
   return NodePtr(nullptr);
 }
 

nav2_smac_planner/src/collision_checker.cpp

@@ -130,11 +130,11 @@ bool GridCollisionChecker::inCollision(
 
     // If its inscribed, in collision, or unknown in the middle,
     // no need to even check the footprint, its invalid
-    if (footprint_cost_ == UNKNOWN && !traverse_unknown) {
+    if (footprint_cost_ == UNKNOWN_COST && !traverse_unknown) {
       return true;
     }
 
-    if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {
+    if (footprint_cost_ == INSCRIBED_COST || footprint_cost_ == OCCUPIED_COST) {
       return true;
     }
 
@@ -153,23 +153,23 @@ bool GridCollisionChecker::inCollision(
 
     footprint_cost_ = static_cast<float>(footprintCost(current_footprint));
 
-    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+    if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return footprint_cost_ >= OCCUPIED;
+    return footprint_cost_ >= OCCUPIED_COST;
   } else {
     // if radius, then we can check the center of the cost assuming inflation is used
     footprint_cost_ = static_cast<float>(costmap_->getCost(
         static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
-    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+    if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return footprint_cost_ >= INSCRIBED;
+    return footprint_cost_ >= INSCRIBED_COST;
   }
 }
 
@@ -178,12 +178,12 @@ bool GridCollisionChecker::inCollision(
   const bool & traverse_unknown)
 {
   footprint_cost_ = costmap_->getCost(i);
-  if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+  if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) {
     return false;
   }
 
   // if occupied or unknown and not to traverse unknown space
-  return footprint_cost_ >= INSCRIBED;
+  return footprint_cost_ >= INSCRIBED_COST;
 }
 
 float GridCollisionChecker::getCost()

nav2_smac_planner/src/costmap_downsampler.cpp

@@ -48,7 +48,7 @@ void CostmapDownsampler::on_configure(
 
   _downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>(
     _downsampled_size_x, _downsampled_size_y, _downsampled_resolution,
-    _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN);
+    _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST);
 
   if (!node.expired()) {
     _downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(

nav2_smac_planner/src/node_2d.cpp

@@ -128,15 +128,15 @@ void Node2D::getNeighbors(
   uint64_t index;
   NodePtr neighbor;
   uint64_t node_i = this->getIndex();
-  const Coordinates parent = getCoords(this->getIndex());
+  const Coordinates coord_parent = getCoords(this->getIndex());
   Coordinates child;
 
   for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) {
     index = node_i + _neighbors_grid_offsets[i];
 
     // Check for wrap around conditions
     child = getCoords(index);
-    if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) {
+    if (fabs(coord_parent.x - child.x) > 1 || fabs(coord_parent.y - child.y) > 1) {
       continue;
     }
 

nav2_smac_planner/src/node_hybrid.cpp

@@ -169,9 +169,9 @@ void HybridMotionTable::initDubin(
     const TurnDirection turn_dir = projections[i]._turn_dir;
     if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) {
       // Turning, so length is the arc length
-      const float angle = projections[i]._theta * bin_size;
-      const float turning_rad = delta_dist / (2.0f * sin(angle / 2.0f));
-      travel_costs[i] = turning_rad * angle;
+      const float arc_angle = projections[i]._theta * bin_size;
+      const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f));
+      travel_costs[i] = turning_rad * arc_angle;
     } else {
       travel_costs[i] = delta_dist;
     }
@@ -290,9 +290,9 @@ void HybridMotionTable::initReedsShepp(
     const TurnDirection turn_dir = projections[i]._turn_dir;
     if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) {
       // Turning, so length is the arc length
-      const float angle = projections[i]._theta * bin_size;
-      const float turning_rad = delta_dist / (2.0f * sin(angle / 2.0f));
-      travel_costs[i] = turning_rad * angle;
+      const float arc_angle = projections[i]._theta * bin_size;
+      const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f));
+      travel_costs[i] = turning_rad * arc_angle;
     } else {
       travel_costs[i] = delta_dist;
     }
@@ -305,11 +305,11 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
   projection_list.reserve(projections.size());
 
   for (unsigned int i = 0; i != projections.size(); i++) {
-    const MotionPose & motion_model = projections[i];
+    const MotionPose & proj_motion_model = projections[i];
 
     // normalize theta, I know its overkill, but I've been burned before...
     const float & node_heading = node->pose.theta;
-    float new_heading = node_heading + motion_model._theta;
+    float new_heading = node_heading + proj_motion_model._theta;
 
     if (new_heading < 0.0) {
       new_heading += num_angle_quantization_float;
@@ -322,7 +322,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
     projection_list.emplace_back(
       delta_xs[i][node_heading] + node->pose.x,
       delta_ys[i][node_heading] + node->pose.y,
-      new_heading, motion_model._turn_dir);
+      new_heading, proj_motion_model._turn_dir);
   }
 
   return projection_list;
@@ -567,6 +567,8 @@ float NodeHybrid::getObstacleHeuristic(
   const Coordinates & goal_coords,
   const float & cost_penalty)
 {
+  (void)goal_coords;
+
   // If already expanded, return the cost
   auto costmap = costmap_ros->getCostmap();
   const bool is_circular = costmap_ros->getUseRadius();
@@ -651,8 +653,8 @@ float NodeHybrid::getObstacleHeuristic(
           unsigned int y_offset = (new_idx / size_x) * 2;
           unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2;
           cost = costmap->getCost(x_offset, y_offset);
-          for (unsigned int i = 0; i < 2u; ++i) {
-            unsigned int mxd = x_offset + i;
+          for (unsigned int k = 0; k < 2u; ++k) {
+            unsigned int mxd = x_offset + k;
             if (mxd >= costmap->getSizeInCellsX()) {
               continue;
             }
@@ -661,7 +663,7 @@ float NodeHybrid::getObstacleHeuristic(
               if (myd >= costmap->getSizeInCellsY()) {
                 continue;
               }
-              if (i == 0 && j == 0) {
+              if (k == 0 && j == 0) {
                 continue;
               }
               cost = std::min(cost, static_cast<float>(costmap->getCost(mxd, myd)));
@@ -674,10 +676,10 @@ float NodeHybrid::getObstacleHeuristic(
         if (!is_circular) {
           // Adjust cost value if using SE2 footprint checks
           cost = adjustedFootprintCost(cost);
-          if (cost >= OCCUPIED) {
+          if (cost >= OCCUPIED_COST) {
             continue;
           }
-        } else if (cost >= INSCRIBED) {
+        } else if (cost >= INSCRIBED_COST) {
           continue;
         }
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -288,7 +288,6 @@ void SmacPlannerHybrid::activate()
 
   // Special case handling to obtain resolution changes in global costmap
   auto resolution_remote_cb = [this](const rclcpp::Parameter & p) {
-      auto node = _node.lock();
       dynamicParametersCallback(
         {rclcpp::Parameter("resolution", rclcpp::ParameterValue(p.as_double()))});
     };

nav2_smac_planner/src/smoother.cpp

@@ -170,7 +170,7 @@ bool Smoother::smoothImpl(
         cost = static_cast<float>(costmap->getCost(mx, my));
       }
 
-      if (cost > MAX_NON_OBSTACLE && cost != UNKNOWN) {
+      if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) {
         RCLCPP_DEBUG(
           rclcpp::get_logger("SmacPlannerSmoother"),
           "Smoothing process resulted in an infeasible collision. "
@@ -374,7 +374,7 @@ void Smoother::findBoundaryExpansion(
     // Check for collision
     unsigned int mx, my;
     costmap->worldToMap(x, y, mx, my);
-    if (static_cast<float>(costmap->getCost(mx, my)) >= INSCRIBED) {
+    if (static_cast<float>(costmap->getCost(mx, my)) >= INSCRIBED_COST) {
       expansion.in_collision = true;
     }
 

nav2_smac_planner/test/deprecated/smoother_cost_function.hpp

@@ -398,7 +398,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
     double & r,
     Eigen::Vector2d & xi) const
   {
-    if (value == FREE) {
+    if (value == FREE_COST) {
       return;
     }
 
@@ -432,7 +432,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
     double & j0,
     double & j1) const
   {
-    if (value == FREE) {
+    if (value == FREE_COST) {
       return;
     }
 

nav2_smac_planner/test/test_a_star.cpp

@@ -301,7 +301,6 @@ TEST(AStarTest, test_a_star_lattice)
     nav2_smac_planner::MotionModel::STATE_LATTICE, info);
   int max_iterations = 10000;
   float tolerance = 10.0;
-  int it_on_approach = 10;
   int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;

nav2_smac_planner/test/test_node2d.cpp

@@ -147,6 +147,9 @@ TEST(Node2DTest, test_node_2d_neighbors)
     [&, this](const uint64_t & index,
     nav2_smac_planner::Node2D * & neighbor_rtn) -> bool
     {
+      (void)index;
+      (void)neighbor_rtn;
+
       return false;
     };
 

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -367,6 +367,9 @@ TEST(NodeHybridTest, test_node_reeds_neighbors)
     [&, this](const uint64_t & index,
     nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool
     {
+      (void)index;
+      (void)neighbor_rtn;
+
       // because we don't return a real object
       return false;
     };

nav2_smac_planner/test/test_nodelattice.cpp

@@ -308,6 +308,9 @@ TEST(NodeLatticeTest, test_get_neighbors)
     [&, this](const uint64_t & index,
     nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool
     {
+      (void)index;
+      (void)neighbor_rtn;
+
       // because we don't return a real object
       return false;
     };