diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index a5a84ae5df..1980475aa0 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) // Fortran subroutine in EISPACK. int i, j, k; - double f, g, h, hh; + double f, g, hh; for (j = 0; j < n; j++) { d[j] = V[n - 1][j]; } @@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) for (i = 0; i < n - 1; i++) { V[n - 1][i] = V[i][i]; V[i][i] = 1.0; - h = d[i + 1]; + const double h = d[i + 1]; if (h != 0.0) { for (k = 0; k <= i; k++) { d[k] = V[k][i + 1] / h; diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index c7046c837e..b35a461f94 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -37,7 +37,7 @@ macro(nav2_package) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC ) + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow) add_compile_options("$<$:-Wnon-virtual-dtor>") endif() diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index aa3ba1eb02..526754e8ed 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -194,8 +194,8 @@ class Smoother // update cusp zone costmap weights if (is_cusp) { double len_to_cusp = current_segment_len; - for (int i = potential_cusp_funcs.size() - 1; i >= 0; i--) { - auto & f = potential_cusp_funcs[i]; + for (int i_cusp = potential_cusp_funcs.size() - 1; i_cusp >= 0; i_cusp--) { + auto & f = potential_cusp_funcs[i_cusp]; double new_weight = params.cusp_costmap_weight * (1.0 - len_to_cusp / cusp_half_length) + params.costmap_weight * len_to_cusp / cusp_half_length; diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp index b8f6c427a6..960f83be99 100644 --- a/nav2_costmap_2d/plugins/denoise_layer.cpp +++ b/nav2_costmap_2d/plugins/denoise_layer.cpp @@ -171,12 +171,12 @@ DenoiseLayer::removeSinglePixels(Image & image) const return v == NO_INFORMATION ? FREE_SPACE : v; }; auto max = [&](const std::initializer_list lst) { - std::array buf = { + std::array rbuf = { replace_to_free(*lst.begin()), replace_to_free(*(lst.begin() + 1)), replace_to_free(*(lst.begin() + 2)) }; - return *std::max_element(buf.begin(), buf.end()); + return *std::max_element(rbuf.begin(), rbuf.end()); }; dilate(image, max_neighbors_image, group_connectivity_type_, max); } else { diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index cfafb6eb4e..466fd015c8 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -73,8 +73,8 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber DummyFootprintSubscriber( nav2_util::LifecycleNode::SharedPtr node, std::string & topic_name, - tf2_ros::Buffer & tf_) - : FootprintSubscriber(node, topic_name, tf_) + tf2_ros::Buffer & tf) + : FootprintSubscriber(node, topic_name, tf) {} void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg) diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 3006eb6f77..64757f3169 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -526,8 +526,8 @@ DWBLocalPlanner::transformGlobalPlan( // from the robot using integrated distance auto transformation_end = std::find_if( transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold; + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose.pose) > transform_end_threshold; }); // Transform the near part of the global plan into the robot's frame of reference. diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp index 30d8f47c6a..b556a06be1 100644 --- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -105,8 +105,7 @@ TEST(ObstacleFootprint, GetOrientedFootprint) pose.theta = theta; footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before); - uint i; - for (i = 0; i < footprint_before.size(); i++) { + for (uint i = 0; i < footprint_before.size(); i++) { ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]); } diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 7cc1002892..8259950b03 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -421,11 +421,10 @@ inline void NavFn::updateCell(int n) { // get neighbors - float u, d, l, r; - l = potarr[n - 1]; - r = potarr[n + 1]; - u = potarr[n - nx]; - d = potarr[n + nx]; + const float l = potarr[n - 1]; + const float r = potarr[n + 1]; + const float u = potarr[n - nx]; + const float d = potarr[n + nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost: %d\n", costarr[n]); @@ -452,8 +451,8 @@ NavFn::updateCell(int n) // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide - float d = dc / hf; - float v = -0.2301 * d * d + 0.5307 * d + 0.7040; + const float div = dc / hf; + const float v = -0.2301 * div * div + 0.5307 * div + 0.7040; pot = ta + hf * v; } @@ -496,11 +495,10 @@ inline void NavFn::updateCellAstar(int n) { // get neighbors - float u, d, l, r; - l = potarr[n - 1]; - r = potarr[n + 1]; - u = potarr[n - nx]; - d = potarr[n + nx]; + float l = potarr[n - 1]; + float r = potarr[n + 1]; + float u = potarr[n - nx]; + float d = potarr[n + nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]); @@ -527,8 +525,8 @@ NavFn::updateCellAstar(int n) // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide - float d = dc / hf; - float v = -0.2301 * d * d + 0.5307 * d + 0.7040; + const float div = dc / hf; + const float v = -0.2301 * div * div + 0.5307 * div + 0.7040; pot = ta + hf * v; } @@ -834,22 +832,22 @@ NavFn::calcPath(int n, int * st) // check eight neighbors to find the lowest int minc = stc; int minp = potarr[stc]; - int st = stcpx - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stc - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stc + 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stcnx - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} + int sti = stcpx - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stc - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stc + 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stcnx - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} stc = minc; dx = 0; dy = 0; diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index f4ceec759d..90b7c4a3e0 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -77,8 +77,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose) > max_costmap_extent; + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); // Lambda to transform a PoseStamped from global frame to local diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index d7afe9b55d..f14934af06 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -158,8 +158,8 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber DummyFootprintSubscriber( nav2_util::LifecycleNode::SharedPtr node, const std::string & topic_name, - tf2_ros::Buffer & tf_) - : FootprintSubscriber(node, topic_name, tf_) + tf2_ros::Buffer & tf) + : FootprintSubscriber(node, topic_name, tf) { auto footprint = std::make_shared(); footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e65b2f928d..352f9f2277 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -45,6 +46,7 @@ set(dependencies nav2_navfn_planner angles behaviortree_cpp_v3 + pluginlib ) set(local_controller_plugin_lib local_controller)