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_cspace: fixes memory access error on global map boundary. #49

Merged
merged 23 commits into from
Jul 28, 2017
Merged

costmap_cspace: fixes memory access error on global map boundary. #49

Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
a42d3e9
Refactor costmap_cspace package.
at-wat Jul 25, 2017
8b3eb76
Fix coding styles.
at-wat Jul 25, 2017
36c037d
Merge branch 'master' into cc-refactor
at-wat Jul 25, 2017
969f3ce
Split costmap calculation into a class.
at-wat Jul 25, 2017
0aaf4e2
Fixing Costmap3DOF class structure and adding tests.
at-wat Jul 25, 2017
e9e0e1a
Revert test. (will be in next PR)
at-wat Jul 25, 2017
6ed8f2d
Add funcs to get state and add some asserts.
at-wat Jul 26, 2017
ddff60f
Wrap 3-D array access and add asserts.
at-wat Jul 26, 2017
fbdc47f
Fix variable names and getCostRef access privilege.
at-wat Jul 27, 2017
cfc6e97
Remove ros::Time::now and use stamp of input map.
at-wat Jul 27, 2017
6c2829c
Add a lot of consts.
at-wat Jul 27, 2017
130122b
Add index range check and assert.
at-wat Jul 27, 2017
1d45988
Fix unsigned variable with negative value in grid address calculation.
at-wat Jul 27, 2017
bdc0086
Fix node to follow variable name change (fbdc47).
at-wat Jul 27, 2017
9a0ab94
Fix cost array accessor target (ddff60).
at-wat Jul 27, 2017
112edfb
Add some more asserts.
at-wat Jul 27, 2017
80963e8
Fix invalid use of Vec.
at-wat Jul 27, 2017
7cb766b
Revert "Fix unsigned variable with negative value in grid address cal…
at-wat Jul 27, 2017
592a601
Revert "Fix invalid use of Vec."
at-wat Jul 27, 2017
4db74bc
Fix unsigned variable with negative value in grid address calculation.
at-wat Jul 27, 2017
d0e7ca7
Fix invalid use of Vec.
at-wat Jul 27, 2017
bf4685a
Fix cost array accessor target (ddff60).
at-wat Jul 27, 2017
b5de024
Merge branch 'master' into cc-fix-memory-access-violations
at-wat Jul 28, 2017
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
53 changes: 29 additions & 24 deletions costmap_cspace/include/costmap_3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -420,47 +420,52 @@ class Costmap3DOF
const ros::Time &stamp)
{
costmap_cspace::CSpace3DUpdate update;
update.header = map_.header;
map_.header.stamp = stamp;
update.x = x - range_max_;
update.y = y - range_max_;
update.width = width + range_max_ * 2;
update.height = height + range_max_ * 2;
if (update.x < 0)
update.header = map.header;
map.header.stamp = stamp;
int update_x = x - range_max_;
int update_y = y - range_max_;
int update_width = width + range_max_ * 2;
int update_height = height + range_max_ * 2;
if (update_x < 0)
{
update.width += update.x;
update.x = 0;
update_width += update_x;
update_x = 0;
}
if (update.y < 0)
if (update_y < 0)
{
update.height += update.y;
update.y = 0;
update_height += update_y;
update_y = 0;
}
if (update.x + update.width > map_.info.width)
if (update_x + update_width > map.info.width)
{
update.width = map_.info.width - update.x;
update_width = map.info.width - update_x;
}
if (update.y + update.height > map_.info.height)
if (update_y + update_height > map.info.height)
{
update.height = map_.info.height - update.y;
update_height = map.info.height - update_y;
}
update.x = update_x;
update.y = update_y;
update.width = update_width;
update.height = update_height;
update.yaw = yaw;
update.angle = angle;
update.data.resize(update.width * update.height * update.angle);

costmap_cspace::Vec p;
for (p[0] = 0; p[0] < update.width; p[0]++)
for (int i = 0; i < update.width; i++)
{
for (p[1] = 0; p[1] < update.height; p[1]++)
for (int j = 0; j < update.height; j++)
{
for (p[2] = 0; p[2] < update.angle; p[2]++)
for (int k = 0; k < update.angle; k++)
{
const int x2 = update.x + p[0];
const int y2 = update.y + p[1];
const int yaw2 = yaw + p[2];
const int x2 = update.x + i;
const int y2 = update.y + j;
const int yaw2 = yaw + k;

const auto &m = getCostRef(x2, y2, yaw2, map);
auto &up = update.data[(p[2] * update.height + p[1]) * update.width + p[0]];
const size_t addr = (k * update.height + j) * update.width + i;
assert(addr < update.data.size());
auto &up = update.data[addr];
up = m;
}
}
Expand Down