Skip to content

Commit

Permalink
Use unsigned char instead of non-standard u_char type (ros-navigation…
Browse files Browse the repository at this point in the history
…#4867)

Signed-off-by: Silvio Traversaro <[email protected]>
Signed-off-by: RBT22 <[email protected]>
  • Loading branch information
traversaro authored and RBT22 committed Jan 22, 2025
1 parent 38a62a7 commit a8cfb85
Show file tree
Hide file tree
Showing 3 changed files with 11 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -142,10 +142,10 @@ class Smoother
std::vector<bool> & optimized)
{
// Create costmap grid
costmap_grid_ = std::make_shared<ceres::Grid2D<u_char>>(
costmap_grid_ = std::make_shared<ceres::Grid2D<unsigned char>>(
costmap->getCharMap(), 0, costmap->getSizeInCellsY(), 0, costmap->getSizeInCellsX());
auto costmap_interpolator = std::make_shared<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(
*costmap_grid_);
auto costmap_interpolator =
std::make_shared<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>>(*costmap_grid_);

// Create residual blocks
const double cusp_half_length = params.cusp_zone_length / 2;
Expand Down Expand Up @@ -394,7 +394,7 @@ class Smoother

bool debug_;
ceres::Solver::Options options_;
std::shared_ptr<ceres::Grid2D<u_char>> costmap_grid_;
std::shared_ptr<ceres::Grid2D<unsigned char>> costmap_grid_;
};

} // namespace nav2_constrained_smoother
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ class SmootherCostFunction
double next_to_last_length_ratio,
bool reversing,
const nav2_costmap_2d::Costmap2D * costmap,
const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>> &
costmap_interpolator,
const SmootherParams & params,
double costmap_weight)
: original_pos_(original_pos),
Expand Down Expand Up @@ -244,7 +245,7 @@ class SmootherCostFunction
double costmap_weight_;
Eigen::Vector2d costmap_origin_;
double costmap_resolution_;
std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> costmap_interpolator_;
std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>> costmap_interpolator_;
};

} // namespace nav2_constrained_smoother
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>> &
costmap_interpolator,
const nav2_constrained_smoother::SmootherParams & params,
double costmap_weight)
: SmootherCostFunction(
Expand Down Expand Up @@ -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<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(),
&costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>>(),
nav2_constrained_smoother::SmootherParams(), 0.0
);

Expand All @@ -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<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(),
&costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char>>>(),
params_no_min_turning_radius, 0.0
);
EXPECT_EQ(fn_no_min_turning_radius.getCurvatureResidual(1.0, pt, pt_other, pt_other), 0.0);
Expand Down

0 comments on commit a8cfb85

Please sign in to comment.