Skip to content

Commit

Permalink
costmap_cspace: make Costmap3dLayerPlain and Costmap3dLayerOutput fas…
Browse files Browse the repository at this point in the history 
…ter (#562)
  • Loading branch information
@nhatao
nhatao committed Nov 12, 2020
1 parent 2a66890 commit 4ec97a0
Show file tree
Hide file tree
Showing 5 changed files with 225 additions and 105 deletions.
14 changes: 14 additions & 0 deletions costmap_cspace/include/costmap_cspace/costmap_3d_layer/base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,20 @@ class CSpace3DMsg : public costmap_cspace_msgs::CSpace3D

return data[addr];
}
static void copyCells(CSpace3DMsg& to, const int& to_x, const int& to_y, const int& to_yaw,
const CSpace3DMsg& from, const int& from_x, const int& from_y, const int& from_yaw,
const int& copy_cell_num)
{
std::memcpy(to.data.data() + to.address(to_x, to_y, to_yaw),
from.data.data() + from.address(from_x, from_y, from_yaw), copy_cell_num * sizeof(int8_t));
}
static void copyCells(costmap_cspace_msgs::CSpace3DUpdate& to, const int& to_x, const int& to_y, const int& to_yaw,
const CSpace3DMsg& from, const int& from_x, const int& from_y, const int& from_yaw,
const int& copy_cell_num)
{
std::memcpy(to.data.data() + (to_yaw * to.height + to_y) * to.width + to_x,
from.data.data() + from.address(from_x, from_y, from_yaw), copy_cell_num * sizeof(int8_t));
}
};

enum MapOverlayMode
Expand Down
194 changes: 108 additions & 86 deletions costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,10 @@

#include <ros/ros.h>

#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <costmap_cspace_msgs/CSpace3D.h>
#include <costmap_cspace_msgs/CSpace3DUpdate.h>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>

#include <xmlrpcpp/XmlRpcValue.h>

Expand All @@ -65,6 +65,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase

CSpace3Cache cs_template_;
int range_max_;
std::vector<bool> unknown_buf_;

public:
Costmap3dLayerFootprint()
Expand Down Expand Up @@ -188,12 +189,28 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
else
generateCSpace(map_overlay_, map, region);
}
void generateCSpace(
virtual void generateCSpace(
CSpace3DMsg::Ptr map,
const nav_msgs::OccupancyGrid::ConstPtr& msg,
const UpdatedRegion& region)
{
ROS_ASSERT(ang_grid_ > 0);
clearTravelableArea(map, msg);
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
{
generateSpecifiedCSpace(map, msg, yaw);
}
}

// Clear travelable area in OVERWRITE mode
void clearTravelableArea(
CSpace3DMsg::Ptr map,
const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
if (overlay_mode_ != OVERWRITE || root_)
{
return;
}
const int ox =
std::lround((msg->info.origin.position.x - map->info.origin.position.x) /
map->info.linear_resolution);
Expand All @@ -202,112 +219,117 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
map->info.linear_resolution);
const double resolution_scale = msg->info.resolution / map->info.linear_resolution;

// Clear travelable area in OVERWRITE mode
if (overlay_mode_ == OVERWRITE && !root_)
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
{
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
for (size_t i = 0; i < msg->data.size(); i++)
{
for (size_t i = 0; i < msg->data.size(); i++)
{
const auto& val = msg->data[i];
if (val < 0)
continue;
const auto& val = msg->data[i];
if (val < 0)
continue;

const int x = std::lround((i % msg->info.width) * resolution_scale);
if (x < range_max_ || static_cast<int>(msg->info.width) - range_max_ <= x)
continue;
const int y = std::lround((i / msg->info.width) * resolution_scale);
if (y < range_max_ || static_cast<int>(msg->info.height) - range_max_ <= y)
continue;
const int x = std::lround((i % msg->info.width) * resolution_scale);
if (x < range_max_ || static_cast<int>(msg->info.width) - range_max_ <= x)
continue;
const int y = std::lround((i / msg->info.width) * resolution_scale);
if (y < range_max_ || static_cast<int>(msg->info.height) - range_max_ <= y)
continue;

const int res_up = std::ceil(resolution_scale);
for (int yp = 0; yp < res_up; yp++)
const int res_up = std::ceil(resolution_scale);
for (int yp = 0; yp < res_up; yp++)
{
const int y2 = y + oy + yp;
if (static_cast<size_t>(y2) >= map->info.height)
continue;
for (int xp = 0; xp < res_up; xp++)
{
for (int xp = 0; xp < res_up; xp++)
{
const int x2 = x + ox + xp;
const int y2 = y + oy + yp;
if (static_cast<size_t>(x2) >= map->info.width ||
static_cast<size_t>(y2) >= map->info.height)
continue;
const int x2 = x + ox + xp;
if (static_cast<size_t>(x2) >= map->info.width)
continue;

map->getCost(x2, y2, yaw) = -1;
}
map->getCost(x2, y2, yaw) = -1;
}
}
}
}
std::vector<bool> unknown;
// Get max
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
}
void generateSpecifiedCSpace(
CSpace3DMsg::Ptr map,
const nav_msgs::OccupancyGrid::ConstPtr& msg,
const size_t yaw)
{
const int ox =
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);
const double resolution_scale = msg->info.resolution / map->info.linear_resolution;
if (keep_unknown_)
{
if (keep_unknown_)
{
unknown.resize(msg->data.size());
for (size_t i = 0; i < msg->data.size(); i++)
{
const int gx = std::lround((i % msg->info.width) * resolution_scale) + ox;
const int gy = std::lround((i / msg->info.width) * resolution_scale) + oy;
if (static_cast<size_t>(gx) >= map->info.width ||
static_cast<size_t>(gy) >= map->info.height)
continue;
// If the cell is unknown in parent map and also updated map,
// the cell is never measured.
// Never measured cells should be marked unknown
// to be correctly processed in the planner.
unknown[i] = msg->data[i] < 0 && map->getCost(gx, gy, yaw) < 0;
}
}
unknown_buf_.resize(msg->data.size());
for (size_t i = 0; i < msg->data.size(); i++)
{
const int gx = std::lround((i % msg->info.width) * resolution_scale) + ox;
const int gy = std::lround((i / msg->info.width) * resolution_scale) + oy;
if (static_cast<size_t>(gx) >= map->info.width ||
static_cast<size_t>(gy) >= map->info.height)
continue;
const int8_t val = msg->data[i];
if (val < 0)
{
continue;
}
else if (val == 0)
{
int8_t& m = map->getCost(gx, gy, yaw);
if (m < 0)
m = 0;
continue;
}
// If the cell is unknown in parent map and also updated map,
// the cell is never measured.
// Never measured cells should be marked unknown
// to be correctly processed in the planner.
unknown_buf_[i] = msg->data[i] < 0 && map->getCost(gx, gy, yaw) < 0;
}
}
for (size_t i = 0; i < msg->data.size(); i++)
{
const int8_t val = msg->data[i];
if (val < 0)
{
continue;
}
const int gx = std::lround((i % msg->info.width) * resolution_scale) + ox;
const int gy = std::lround((i / msg->info.width) * resolution_scale) + oy;
if (static_cast<size_t>(gx) >= map->info.width ||
static_cast<size_t>(gy) >= map->info.height)
continue;
if (val == 0)
{
int8_t& m = map->getCost(gx, gy, yaw);
if (m < 0)
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);
for (int map_y = map_y_min; map_y <= map_y_max; ++map_y)
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);
for (int map_y = map_y_min; map_y <= map_y_max; ++map_y)
{
// Use raw pointers for faster iteration
int8_t* cost_addr = &(map->getCost(map_x_min, map_y, yaw));
const char* cs_addr = &(cs_template_.e(map_x_min - gx, map_y - gy, yaw));
for (int n = 0; n <= map_x_max - map_x_min; ++n, ++cost_addr, ++cs_addr)
{
// Use raw pointers for faster iteration
int8_t* cost_addr = &(map->getCost(map_x_min, map_y, yaw));
const char* cs_addr = &(cs_template_.e(map_x_min - gx, map_y - gy, yaw));
for (int n = 0; n <= map_x_max - map_x_min; ++n, ++cost_addr, ++cs_addr)
{
const int8_t c = *cs_addr * val / 100;
if (c > 0 && *cost_addr < c)
*cost_addr = c;
}
const int8_t c = *cs_addr * val / 100;
if (c > 0 && *cost_addr < c)
*cost_addr = c;
}
}
if (keep_unknown_)
}
if (keep_unknown_)
{
for (size_t i = 0; i < unknown_buf_.size(); i++)
{
for (size_t i = 0; i < unknown.size(); i++)
{
if (!unknown[i])
continue;
const int gx = std::lround((i % msg->info.width) * resolution_scale) + ox;
const int gy = std::lround((i / msg->info.width) * resolution_scale) + oy;
if (static_cast<size_t>(gx) >= map->info.width ||
static_cast<size_t>(gy) >= map->info.height)
continue;
map->getCost(gx, gy, yaw) = -1;
}
if (!unknown_buf_[i])
continue;
const int gx = std::lround((i % msg->info.width) * resolution_scale) + ox;
const int gy = std::lround((i / msg->info.width) * resolution_scale) + oy;
if (static_cast<size_t>(gx) >= map->info.width ||
static_cast<size_t>(gy) >= map->info.height)
continue;
map->getCost(gx, gy, yaw) = -1;
}
}
}
Expand Down
30 changes: 13 additions & 17 deletions costmap_cspace/include/costmap_cspace/costmap_3d_layer/output.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,10 @@

#include <memory>

#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <costmap_cspace_msgs/CSpace3D.h>
#include <costmap_cspace_msgs/CSpace3DUpdate.h>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>

#include <costmap_cspace/costmap_3d_layer/base.h>

Expand Down Expand Up @@ -106,23 +106,19 @@ class Costmap3dLayerOutput : public Costmap3dLayerBase
update_msg->yaw = region_merged.yaw_;
update_msg->angle = region_merged.angle_;
update_msg->data.resize(update_msg->width * update_msg->height * update_msg->angle);

for (int i = 0; i < static_cast<int>(update_msg->width); i++)
if ((update_msg->x == 0) && (update_msg->y == 0) && (update_msg->yaw == 0) &&
(update_msg->width == map_->info.width) && (update_msg->height == map_->info.height) &&
(update_msg->angle == map_->info.angle))
{
CSpace3DMsg::copyCells(*update_msg, 0, 0, 0, *map_, 0, 0, 0, update_msg->data.size());
return update_msg;
}
for (unsigned int k = 0; k < update_msg->angle; ++k)
{
for (int j = 0; j < static_cast<int>(update_msg->height); j++)
for (unsigned int j = 0; j < update_msg->height; ++j)
{
for (int k = 0; k < static_cast<int>(update_msg->angle); k++)
{
const int x2 = update_msg->x + i;
const int y2 = update_msg->y + j;
const int yaw2 = update_msg->yaw + k;

const auto& m = map_->getCost(x2, y2, yaw2);
const size_t addr = (k * update_msg->height + j) * update_msg->width + i;
ROS_ASSERT(addr < update_msg->data.size());
auto& up = update_msg->data[addr];
up = m;
}
CSpace3DMsg::copyCells(*update_msg, 0, j, k,
*map_, update_msg->x, update_msg->y + j, update_msg->yaw + k, update_msg->width);
}
}
return update_msg;
Expand Down
20 changes: 18 additions & 2 deletions costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,13 @@

#include <memory>

#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <costmap_cspace_msgs/CSpace3D.h>
#include <costmap_cspace_msgs/CSpace3DUpdate.h>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>

#include <costmap_cspace/costmap_3d_layer/base.h>
#include <costmap_cspace/costmap_3d_layer/footprint.h>

namespace costmap_cspace
{
Expand All @@ -62,6 +63,21 @@ class Costmap3dLayerPlain : public Costmap3dLayerFootprint
static_cast<double>(config["linear_expand"]),
static_cast<double>(config["linear_spread"]));
}

protected:
void generateCSpace(
CSpace3DMsg::Ptr map,
const nav_msgs::OccupancyGrid::ConstPtr& msg,
const UpdatedRegion& region) final
{
ROS_ASSERT(ang_grid_ > 0);
clearTravelableArea(map, msg);
generateSpecifiedCSpace(map, msg, 0);
for (size_t i = 1; i < map->info.angle; ++i)
{
CSpace3DMsg::copyCells(*map, 0, 0, i, *map, 0, 0, 0, map->info.width * map->info.height);
}
}
};
} // namespace costmap_cspace

Expand Down
Loading

0 comments on commit 4ec97a0

Please sign in to comment.