Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

costmap_3d: avoid unnecessary grid expansions #726

Merged
merged 8 commits into from
Nov 1, 2023
Merged

costmap_3d: avoid unnecessary grid expansions #726

Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
edc873a
costmap_3d: avoid unnecessary grid expansions
nhatao Oct 27, 2023
2d1fc0b
Fix algorithm
nhatao Oct 29, 2023
7840707
Remove unnecessary member
nhatao Oct 31, 2023
40ad6a1
Merge branch 'master' of https://github.com/at-wat/neonavigation into…
nhatao Oct 31, 2023
e55b0e8
Remove template_range
nhatao Nov 1, 2023
28e0e72
Avoid using member function
nhatao Nov 1, 2023
2b4c132
Fix variable name
nhatao Nov 1, 2023
c3e88be
Fix unintended changes
nhatao Nov 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 28 additions & 9 deletions costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,10 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
bool keep_unknown_;

CSpace3Cache cs_template_;
struct Rect
{
int x_min, x_max, y_min, y_max;
};
int range_max_;
std::vector<bool> unknown_buf_;

Expand Down Expand Up @@ -266,17 +270,31 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
}
}
}

void generateSpecifiedCSpace(
CSpace3DMsg::Ptr map,
const nav_msgs::OccupancyGrid::ConstPtr& msg,
const size_t yaw)
{
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<int>(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<int>(msg->info.height) - 1 || (*(ptr + msg->info.width) >= *ptr)) ? 0 : range_max_;
};
const auto getRange = (msg->info.resolution == map->info.linear_resolution) ?
getMaskedRange :
std::function<void(const int, Rect&)>([](const int, Rect&) {});

const int ox =
std::lround((msg->info.origin.position.x - map->info.origin.position.x) /
map->info.linear_resolution);
std::lround((msg->info.origin.position.x - map->info.origin.position.x) / map->info.linear_resolution);
const int oy =
std::lround((msg->info.origin.position.y - map->info.origin.position.y) /
map->info.linear_resolution);
std::lround((msg->info.origin.position.y - map->info.origin.position.y) / map->info.linear_resolution);
const double resolution_scale = msg->info.resolution / map->info.linear_resolution;
if (keep_unknown_)
{
Expand All @@ -295,6 +313,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
unknown_buf_[i] = msg->data[i] < 0 && map->getCost(gx, gy, yaw) < 0;
}
}
Rect range = {-range_max_, range_max_, -range_max_, range_max_};
for (size_t i = 0; i < msg->data.size(); i++)
{
const int8_t val = msg->data[i];
Expand All @@ -314,11 +333,11 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
m = 0;
continue;
}

const int map_x_min = std::max(gx - range_max_, 0);
const int map_x_max = std::min(gx + range_max_, static_cast<int>(map->info.width) - 1);
const int map_y_min = std::max(gy - range_max_, 0);
const int map_y_max = std::min(gy + range_max_, static_cast<int>(map->info.height) - 1);
getRange(i, range);
const int map_x_min = std::max(gx + range.x_min, 0);
const int map_x_max = std::min(gx + range.x_max, static_cast<int>(map->info.width) - 1);
const int map_y_min = std::max(gy + range.y_min, 0);
const int map_y_max = std::min(gy + range.y_max, static_cast<int>(map->info.height) - 1);
for (int map_y = map_y_min; map_y <= map_y_max; ++map_y)
{
// Use raw pointers for faster iteration
Expand Down