From 28e0e72a844bd1beacc1177f623be9d8f54de28c Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Wed, 1 Nov 2023 18:04:48 +0900 Subject: [PATCH] Avoid using member function --- .../costmap_3d_layer/footprint.h | 38 ++++++++----------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h index ad1f560e..9ff63230 100644 --- a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h +++ b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h @@ -271,34 +271,26 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase } } - const void getDefaultRange(const nav_msgs::OccupancyGrid::ConstPtr&, const int, Rect&) const - { - // Do nothing - } - - const void getMaskedRange(const nav_msgs::OccupancyGrid::ConstPtr& msg, const int pos, Rect& result) const - { - const int gx = pos % msg->info.width; - const int gy = pos / msg->info.width; - const int8_t* const ptr = msg->data.data() + pos; - result.x_min = (gx == 0 || (*(ptr - 1) >= *ptr)) ? 0 : -range_max_; - result.x_max = (gx == static_cast(msg->info.width) - 1 || (*(ptr + 1) >= *ptr)) ? 0 : range_max_; - result.y_min = (gy == 0 || (*(ptr - msg->info.width) >= *ptr)) ? 0 : -range_max_; - result.y_max = - (gy == static_cast(msg->info.height) - 1 || (*(ptr + msg->info.width) >= *ptr)) ? 0 : range_max_; - } - void generateSpecifiedCSpace( CSpace3DMsg::Ptr map, const nav_msgs::OccupancyGrid::ConstPtr& msg, const size_t yaw) { - const auto range_getter = - (msg->info.resolution == map->info.linear_resolution) ? - std::bind(&Costmap3dLayerFootprint::getMaskedRange, this, msg, std::placeholders::_1, - std::placeholders::_2) : - std::bind(&Costmap3dLayerFootprint::getDefaultRange, this, msg, std::placeholders::_1, - std::placeholders::_2); + const auto getMaskedRange = [this, msg](const int pos, Rect& result) + { + const int gx = pos % msg->info.width; + const int gy = pos / msg->info.width; + const int8_t* const ptr = msg->data.data() + pos; + result.x_min = (gx == 0 || (*(ptr - 1) >= *ptr)) ? 0 : -range_max_; + result.x_max = (gx == static_cast(msg->info.width) - 1 || (*(ptr + 1) >= *ptr)) ? 0 : range_max_; + result.y_min = (gy == 0 || (*(ptr - msg->info.width) >= *ptr)) ? 0 : -range_max_; + result.y_max = + (gy == static_cast(msg->info.height) - 1 || (*(ptr + msg->info.width) >= *ptr)) ? 0 : range_max_; + }; + const auto range_getter = (msg->info.resolution == map->info.linear_resolution) ? + getMaskedRange : + std::function([](const int, Rect&) {}); + const int ox = std::lround((msg->info.origin.position.x - map->info.origin.position.x) / map->info.linear_resolution); const int oy =