Skip to content

Commit

Permalink
planner_cspace: fix map update range (#672)
Browse files Browse the repository at this point in the history 
- Fix memory access violation
- Fix map_update_max to contain upper boundary edge pixels
  Word definitions:
    - max: contains the value
    - size: does not contain the value

Co-authored-by: Kazuki Takahashi <tkhsh.schoen@gmail.com>
  • Loading branch information
@at-wat @Taka-Kazu
at-wat and Taka-Kazu committed Feb 24, 2023
1 parent d38b46e commit 2564c72
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 32 deletions.
12 changes: 6 additions & 6 deletions planner_cspace/include/planner_cspace/blockmem_gridmap.h
View file
Expand Up @@ -129,11 +129,11 @@ class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
const T zero, const CyclicVecInt<DIM, NONCYCLIC>& min, const CyclicVecInt<DIM, NONCYCLIC>& max)
{
CyclicVecInt<DIM, NONCYCLIC> p = min;
for (p[0] = min[0]; p[0] < max[0]; ++p[0])
for (p[0] = min[0]; p[0] <= max[0]; ++p[0])
{
for (p[1] = min[1]; p[1] < max[1]; ++p[1])
for (p[1] = min[1]; p[1] <= max[1]; ++p[1])
{
for (p[2] = min[2]; p[2] < max[2]; ++p[2])
for (p[2] = min[2]; p[2] <= max[2]; ++p[2])
{
(*this)[p] = zero;
}
Expand All @@ -145,11 +145,11 @@ class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
const CyclicVecInt<DIM, NONCYCLIC>& max)
{
CyclicVecInt<DIM, NONCYCLIC> p = min;
for (p[0] = min[0]; p[0] < max[0]; ++p[0])
for (p[0] = min[0]; p[0] <= max[0]; ++p[0])
{
for (p[1] = min[1]; p[1] < max[1]; ++p[1])
for (p[1] = min[1]; p[1] <= max[1]; ++p[1])
{
for (p[2] = min[2]; p[2] < max[2]; ++p[2])
for (p[2] = min[2]; p[2] <= max[2]; ++p[2])
{
(*this)[p] = base[p];
}
Expand Down
53 changes: 33 additions & 20 deletions planner_cspace/src/planner_3d.cpp
View file
Expand Up @@ -700,12 +700,12 @@ class Planner3dNode
const ros::Time now = ros::Time::now();

const int map_update_x_min = static_cast<int>(msg->x);
const int map_update_x_max = static_cast<int>(msg->x + msg->width);
const int map_update_x_max = std::max(static_cast<int>(msg->x + msg->width) - 1, 0);
const int map_update_y_min = static_cast<int>(msg->y);
const int map_update_y_max = static_cast<int>(msg->y + msg->height);
const int map_update_y_max = std::max(static_cast<int>(msg->y + msg->height) - 1, 0);

if (static_cast<size_t>(map_update_x_max) > map_info_.width ||
static_cast<size_t>(map_update_y_max) > map_info_.height ||
if (static_cast<size_t>(map_update_x_max) >= map_info_.width ||
static_cast<size_t>(map_update_y_max) >= map_info_.height ||
msg->angle > map_info_.angle)
{
ROS_WARN(
Expand All @@ -719,21 +719,34 @@ class Planner3dNode

last_costmap_ = now;

cm_.copy_partially(cm_base_, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, static_cast<int>(map_info_.angle)));
cm_rough_.copy_partially(cm_rough_base_, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 1));
cm_updates_.clear_partially(-1, Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 1));
cm_.copy_partially(
cm_base_,
Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, static_cast<int>(map_info_.angle) - 1));
cm_rough_.copy_partially(
cm_rough_base_,
Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));
cm_updates_.clear_partially(
-1,
Astar::Vec(prev_map_update_x_min_, prev_map_update_y_min_, 0),
Astar::Vec(prev_map_update_x_max_, prev_map_update_y_max_, 0));

// Should search 1px around the region to
// update the costmap even if the edge of the local map is obstacle
const int search_range_x_min = std::max(0, std::min(prev_map_update_x_min_, map_update_x_min) - 1);
const int search_range_x_max = std::min(static_cast<int>(map_info_.width),
std::max(prev_map_update_x_max_, map_update_x_max) + 1);
const int search_range_y_min = std::max(0, std::min(prev_map_update_y_min_, map_update_y_min) - 1);
const int search_range_y_max = std::min(static_cast<int>(map_info_.height),
std::max(prev_map_update_y_max_, map_update_y_max) + 1);
const int search_range_x_min = std::max(
0,
std::min(prev_map_update_x_min_, map_update_x_min) - 1);
const int search_range_x_max = std::min(
static_cast<int>(map_info_.width - 1),
std::max(prev_map_update_x_max_, map_update_x_max) + 1);
const int search_range_y_min = std::max(
0,
std::min(prev_map_update_y_min_, map_update_y_min) - 1);
const int search_range_y_max = std::min(
static_cast<int>(map_info_.height - 1),
std::max(prev_map_update_y_max_, map_update_y_max) + 1);

prev_map_update_x_min_ = map_update_x_min;
prev_map_update_x_max_ = map_update_x_max;
prev_map_update_y_min_ = map_update_y_min;
Expand Down Expand Up @@ -965,10 +978,10 @@ class Planner3dNode
cm_base_ = cm_;
bbf_costmap_.clear();

prev_map_update_x_min_ = std::numeric_limits<int>::max();
prev_map_update_x_max_ = std::numeric_limits<int>::lowest();
prev_map_update_y_min_ = std::numeric_limits<int>::max();
prev_map_update_y_max_ = std::numeric_limits<int>::lowest();
prev_map_update_x_min_ = map_info_.width;
prev_map_update_x_max_ = 0;
prev_map_update_y_min_ = map_info_.height;
prev_map_update_y_max_ = 0;

updateGoal();

Expand Down
12 changes: 6 additions & 6 deletions planner_cspace/test/src/test_blockmem_gridmap.cpp
View file
Expand Up @@ -117,9 +117,9 @@ TEST(BlockmemGridmap, ClearAndCopyPartially)
{
for (p[2] = 0; p[2] < base_size[2]; ++p[2])
{
if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
(copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
(copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
if ((copy_min_pos[0] <= p[0]) && (p[0] <= copy_max_pos[0]) &&
(copy_min_pos[1] <= p[1]) && (p[1] <= copy_max_pos[1]) &&
(copy_min_pos[2] <= p[2]) && (p[2] <= copy_max_pos[2]))
{
EXPECT_EQ(gm[p], -1) << p[0] << "," << p[1] << "," << p[2];
}
Expand Down Expand Up @@ -153,9 +153,9 @@ TEST(BlockmemGridmap, ClearAndCopyPartially)
{
for (p[2] = 0; p[2] < base_size[2]; ++p[2])
{
if ((copy_min_pos[0] <= p[0]) && (p[0] < copy_max_pos[0]) &&
(copy_min_pos[1] <= p[1]) && (p[1] < copy_max_pos[1]) &&
(copy_min_pos[2] <= p[2]) && (p[2] < copy_max_pos[2]))
if ((copy_min_pos[0] <= p[0]) && (p[0] <= copy_max_pos[0]) &&
(copy_min_pos[1] <= p[1]) && (p[1] <= copy_max_pos[1]) &&
(copy_min_pos[2] <= p[2]) && (p[2] <= copy_max_pos[2]))
{
ASSERT_EQ(gm[p], gm_update[p]);
}
Expand Down

0 comments on commit 2564c72

Please sign in to comment.