diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index a0d5dfa115e..f37b40ac3bc 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -142,10 +142,10 @@ class Smoother std::vector & optimized) { // Create costmap grid - costmap_grid_ = std::make_shared>( + costmap_grid_ = std::make_shared>( costmap->getCharMap(), 0, costmap->getSizeInCellsY(), 0, costmap->getSizeInCellsX()); - auto costmap_interpolator = std::make_shared>>( - *costmap_grid_); + auto costmap_interpolator = + std::make_shared>>(*costmap_grid_); // Create residual blocks const double cusp_half_length = params.cusp_zone_length / 2; @@ -394,7 +394,7 @@ class Smoother bool debug_; ceres::Solver::Options options_; - std::shared_ptr> costmap_grid_; + std::shared_ptr> costmap_grid_; }; } // namespace nav2_constrained_smoother diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp index 7253119721c..adfae2bc242 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp @@ -57,7 +57,8 @@ class SmootherCostFunction double next_to_last_length_ratio, bool reversing, const nav2_costmap_2d::Costmap2D * costmap, - const std::shared_ptr>> & costmap_interpolator, + const std::shared_ptr>> & + costmap_interpolator, const SmootherParams & params, double costmap_weight) : original_pos_(original_pos), @@ -244,7 +245,7 @@ class SmootherCostFunction double costmap_weight_; Eigen::Vector2d costmap_origin_; double costmap_resolution_; - std::shared_ptr>> costmap_interpolator_; + std::shared_ptr>> costmap_interpolator_; }; } // namespace nav2_constrained_smoother diff --git a/nav2_constrained_smoother/test/test_smoother_cost_function.cpp b/nav2_constrained_smoother/test/test_smoother_cost_function.cpp index 104b949c4be..24904ef5c55 100644 --- a/nav2_constrained_smoother/test/test_smoother_cost_function.cpp +++ b/nav2_constrained_smoother/test/test_smoother_cost_function.cpp @@ -33,7 +33,8 @@ class TestableSmootherCostFunction : nav2_constrained_smoother::SmootherCostFunc double next_to_last_length_ratio, bool reversing, const nav2_costmap_2d::Costmap2D * costmap, - const std::shared_ptr>> & costmap_interpolator, + const std::shared_ptr>> & + costmap_interpolator, const nav2_constrained_smoother::SmootherParams & params, double costmap_weight) : SmootherCostFunction( @@ -68,7 +69,7 @@ TEST_F(Test, testingCurvatureResidual) nav2_costmap_2d::Costmap2D costmap; TestableSmootherCostFunction fn( Eigen::Vector2d(1.0, 0.0), 1.0, false, - &costmap, std::shared_ptr>>(), + &costmap, std::shared_ptr>>(), nav2_constrained_smoother::SmootherParams(), 0.0 ); @@ -81,7 +82,7 @@ TEST_F(Test, testingCurvatureResidual) params_no_min_turning_radius.max_curvature = 1.0f / 0.0; TestableSmootherCostFunction fn_no_min_turning_radius( Eigen::Vector2d(1.0, 0.0), 1.0, false, - &costmap, std::shared_ptr>>(), + &costmap, std::shared_ptr>>(), params_no_min_turning_radius, 0.0 ); EXPECT_EQ(fn_no_min_turning_radius.getCurvatureResidual(1.0, pt, pt_other, pt_other), 0.0);