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: refactor costmap layer classes #100

Merged
merged 8 commits into from
Jan 22, 2018
Merged

costmap_cspace: refactor costmap layer classes #100

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
5962e14
Move cost accessor to accessor class and remove self namespace
at-wat Jan 19, 2018
857ad88
Add layer type flag
at-wat Jan 22, 2018
804001e
Add layer type check
at-wat Jan 22, 2018
712eafb
Revert "Add layer type flag"
at-wat Jan 22, 2018
6fcc51d
Revert "Add layer type check"
at-wat Jan 22, 2018
bf69153
Use ROS_ASSERT instead of assert
at-wat Jan 22, 2018
3718a1a
fix const
at-wat Jan 22, 2018
7e14986
Remove spin function
at-wat Jan 22, 2018
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
101 changes: 50 additions & 51 deletions costmap_cspace/include/costmap_3d_layer_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
#ifndef COSTMAP_3D_LAYER_BASE_H
#define COSTMAP_3D_LAYER_BASE_H

#include <ros/ros.h>

#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <costmap_cspace/CSpace3D.h>
Expand All @@ -39,6 +41,33 @@

namespace costmap_cspace
{
class CSpace3DMsg : public CSpace3D
{
public:
using Ptr = std::shared_ptr<CSpace3DMsg>;
const int8_t &getCost(const int &x, const int &y, const int &yaw) const
{
ROS_ASSERT(static_cast<size_t>(yaw) < info.angle);
ROS_ASSERT(static_cast<size_t>(x) < info.width);
ROS_ASSERT(static_cast<size_t>(y) < info.height);

const size_t addr = (yaw * info.height + y) * info.width + x;
ROS_ASSERT(addr < data.size());

return data[addr];
}
int8_t &getCost(const int &x, const int &y, const int &yaw)
{
ROS_ASSERT(static_cast<size_t>(yaw) < info.angle);
ROS_ASSERT(static_cast<size_t>(x) < info.width);
ROS_ASSERT(static_cast<size_t>(y) < info.height);

const size_t addr = (yaw * info.height + y) * info.width + x;
ROS_ASSERT(addr < data.size());

return data[addr];
}
};
class Costmap3dLayerBase
{
public:
Expand All @@ -56,11 +85,11 @@ class Costmap3dLayerBase
float linear_spread_;
map_overlay_mode overlay_mode_;

costmap_cspace::CSpace3Cache cs_;
CSpace3Cache cs_template_;
int range_max_;

costmap_cspace::CSpace3D::Ptr map_;
costmap_cspace::CSpace3D::Ptr map_overlay_;
CSpace3DMsg::Ptr map_;
CSpace3DMsg::Ptr map_overlay_;
nav_msgs::OccupancyGrid map_base_;

Costmap3dLayerBase::Ptr child_;
Expand All @@ -71,8 +100,8 @@ class Costmap3dLayerBase
, linear_expand_(0.0)
, linear_spread_(0.0)
, overlay_mode_(map_overlay_mode::MAX)
, map_(new costmap_cspace::CSpace3D)
, map_overlay_(new costmap_cspace::CSpace3D)
, map_(new CSpace3DMsg)
, map_overlay_(new CSpace3DMsg)
{
}
void setCSpaceConfig(
Expand All @@ -86,10 +115,10 @@ class Costmap3dLayerBase
linear_spread_ = linear_spread;
overlay_mode_ = overlay_mode;
}
virtual void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info) = 0;
virtual void generateCSpaceTemplate(const MapMetaData3D &info) = 0;
void setBaseMap(const nav_msgs::OccupancyGrid &base_map)
{
assert(base_map.data.size() >= base_map.info.width * base_map.info.height);
ROS_ASSERT(base_map.data.size() >= base_map.info.width * base_map.info.height);
map_base_ = base_map;

map_->header = map_base_.header;
Expand Down Expand Up @@ -130,7 +159,7 @@ class Costmap3dLayerBase
child_ = child;
child_->setMap(getMapOverlay());
}
costmap_cspace::CSpace3DUpdate processMapOverlay(const nav_msgs::OccupancyGrid &msg)
CSpace3DUpdate processMapOverlay(const nav_msgs::OccupancyGrid &msg)
{
const int ox = lroundf((msg.info.origin.position.x - map_->info.origin.position.x) / map_->info.linear_resolution);
const int oy = lroundf((msg.info.origin.position.y - map_->info.origin.position.y) / map_->info.linear_resolution);
Expand All @@ -141,21 +170,21 @@ class Costmap3dLayerBase
const int h = lroundf(msg.info.height * msg.info.resolution / map_->info.linear_resolution);
return generateUpdateMsg(map_overlay_, ox, oy, 0, w, h, map_->info.angle, msg.header.stamp);
}
costmap_cspace::CSpace3D::Ptr getMap()
CSpace3DMsg::Ptr getMap()
{
return map_;
}
void setMap(costmap_cspace::CSpace3D::Ptr map)
void setMap(CSpace3DMsg::Ptr map)
{
map_ = map;
}
costmap_cspace::CSpace3D::Ptr getMapOverlay()
CSpace3DMsg::Ptr getMapOverlay()
{
return map_overlay_;
}
CSpace3Cache &getTemplate()
{
return cs_;
return cs_template_;
}
int getRangeMax() const
{
Expand All @@ -165,36 +194,6 @@ class Costmap3dLayerBase
{
return ang_grid_;
}
int8_t getCost(const int &x, const int &y, const int &yaw) const
{
return getCost(x, y, yaw, map_overlay_);
}
int8_t getCost(const int &x, const int &y, const int &yaw, costmap_cspace::CSpace3D::ConstPtr map) const
{
assert(static_cast<size_t>(yaw) < map->info.angle);
assert(static_cast<size_t>(x) < map->info.width);
assert(static_cast<size_t>(y) < map->info.height);

const size_t addr = (yaw * map->info.height + y) * map->info.width + x;
assert(addr < map->data.size());

return map->data[addr];
}
int8_t &getCostRef(const int &x, const int &y, const int &yaw)
{
return getCostRef(x, y, yaw, map_overlay_);
}
int8_t &getCostRef(const int &x, const int &y, const int &yaw, costmap_cspace::CSpace3D::Ptr map)
{
assert(static_cast<size_t>(yaw) < map->info.angle);
assert(static_cast<size_t>(x) < map->info.width);
assert(static_cast<size_t>(y) < map->info.height);

const size_t addr = (yaw * map->info.height + y) * map->info.width + x;
assert(addr < map->data.size());

return map->data[addr];
}

protected:
void setBaseMapChain()
Expand All @@ -204,7 +203,7 @@ class Costmap3dLayerBase
if (child_)
child_->setBaseMapChain();
}
void gemerateCSpace(costmap_cspace::CSpace3D::Ptr map, const nav_msgs::OccupancyGrid &msg, bool overlay = false)
void gemerateCSpace(CSpace3DMsg::Ptr map, const nav_msgs::OccupancyGrid &msg, bool overlay = false)
{
const int ox = lroundf((msg.info.origin.position.x - map->info.origin.position.x) / map->info.linear_resolution);
const int oy = lroundf((msg.info.origin.position.y - map->info.origin.position.y) / map->info.linear_resolution);
Expand Down Expand Up @@ -237,7 +236,7 @@ class Costmap3dLayerBase
static_cast<unsigned int>(y2) >= map->info.height)
continue;

auto &m = getCostRef(x2, y2, yaw, map);
auto &m = map->getCost(x2, y2, yaw);
m = 0;
}
}
Expand Down Expand Up @@ -269,21 +268,21 @@ class Costmap3dLayerBase
static_cast<unsigned int>(y2) >= map->info.height)
continue;

auto &m = getCostRef(x2, y2, yaw, map);
const auto c = cs_.e(x, y, yaw) * val / 100;
auto &m = map->getCost(x2, y2, yaw);
const auto c = cs_template_.e(x, y, yaw) * val / 100;
if (m < c)
m = c;
}
}
}
}
}
costmap_cspace::CSpace3DUpdate generateUpdateMsg(
costmap_cspace::CSpace3D::Ptr map, const int &x, const int &y, const int &yaw,
CSpace3DUpdate generateUpdateMsg(
CSpace3DMsg::Ptr map, const int &x, const int &y, const int &yaw,
const int &width, const int &height, const int &angle,
const ros::Time &stamp)
{
costmap_cspace::CSpace3DUpdate update;
CSpace3DUpdate update;
update.header = map->header;
map->header.stamp = stamp;
int update_x = x - range_max_;
Expand Down Expand Up @@ -326,9 +325,9 @@ class Costmap3dLayerBase
const int y2 = update.y + j;
const int yaw2 = yaw + k;

const auto &m = getCostRef(x2, y2, yaw2, map);
const auto &m = map->getCost(x2, y2, yaw2);
const size_t addr = (k * update.height + j) * update.width + i;
assert(addr < update.data.size());
ROS_ASSERT(addr < update.data.size());
auto &up = update.data[addr];
up = m;
}
Expand Down
22 changes: 12 additions & 10 deletions costmap_cspace/include/costmap_3d_layer_footprint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
#ifndef COSTMAP_3D_LAYER_FOOTPRINT_H
#define COSTMAP_3D_LAYER_FOOTPRINT_H

#include <ros/ros.h>

#include <geometry_msgs/PolygonStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <costmap_cspace/CSpace3D.h>
Expand Down Expand Up @@ -59,7 +61,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
protected:
float footprint_radius_;
geometry_msgs::PolygonStamped footprint_;
costmap_cspace::Polygon footprint_p_;
Polygon footprint_p_;

public:
Costmap3dLayerFootprint()
Expand All @@ -85,7 +87,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
}

geometry_msgs::Point32 point;
costmap_cspace::Vec v;
Vec v;
v[0] = point.x = static_cast<double>(footprint_xml[i][0]);
v[1] = point.y = static_cast<double>(footprint_xml[i][1]);
point.z = 0;
Expand All @@ -102,13 +104,13 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase

return true;
}
void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info)
void generateCSpaceTemplate(const MapMetaData3D &info)
{
assert(footprint_p_.v.size() > 2);
ROS_ASSERT(footprint_p_.v.size() > 2);

range_max_ =
ceilf((footprint_radius_ + linear_expand_ + linear_spread_) / info.linear_resolution);
cs_.reset(range_max_, range_max_, info.angle);
cs_template_.reset(range_max_, range_max_, info.angle);

// C-Space template
for (size_t yaw = 0; yaw < info.angle; yaw++)
Expand All @@ -121,27 +123,27 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
f.move(x * info.linear_resolution,
y * info.linear_resolution,
yaw * info.angular_resolution);
costmap_cspace::Vec p;
Vec p;
p[0] = 0;
p[1] = 0;
if (f.inside(p))
{
cs_.e(x, y, yaw) = 100;
cs_template_.e(x, y, yaw) = 100;
}
else
{
const float d = f.dist(p);
if (d < linear_expand_)
{
cs_.e(x, y, yaw) = 100;
cs_template_.e(x, y, yaw) = 100;
}
else if (d < linear_expand_ + linear_spread_)
{
cs_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_;
cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_;
}
else
{
cs_.e(x, y, yaw) = 0;
cs_template_.e(x, y, yaw) = 0;
}
}
}
Expand Down
12 changes: 6 additions & 6 deletions costmap_cspace/include/costmap_3d_layer_plain.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,10 @@ class Costmap3dLayerPlain : public Costmap3dLayerBase
Costmap3dLayerPlain()
{
}
void generateCSpaceTemplate(const costmap_cspace::MapMetaData3D &info)
void generateCSpaceTemplate(const MapMetaData3D &info)
{
range_max_ = ceilf((linear_expand_ + linear_spread_) / info.linear_resolution);
cs_.reset(range_max_, range_max_, info.angle);
cs_template_.reset(range_max_, range_max_, info.angle);

// C-Space template
for (size_t yaw = 0; yaw < info.angle; yaw++)
Expand All @@ -62,22 +62,22 @@ class Costmap3dLayerPlain : public Costmap3dLayerBase
{
if (x == 0 && y == 0)
{
cs_.e(x, y, yaw) = 100;
cs_template_.e(x, y, yaw) = 100;
}
else
{
const float d = hypotf(x, y);
if (d < linear_expand_)
{
cs_.e(x, y, yaw) = 100;
cs_template_.e(x, y, yaw) = 100;
}
else if (d < linear_expand_ + linear_spread_)
{
cs_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_;
cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_;
}
else
{
cs_.e(x, y, yaw) = 0;
cs_template_.e(x, y, yaw) = 0;
}
}
}
Expand Down
14 changes: 8 additions & 6 deletions costmap_cspace/include/cspace3_cache.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
#ifndef CSPACE3_CACHE_H
#define CSPACE3_CACHE_H

#include <ros/ros.h>

namespace costmap_cspace
{
class CSpace3Cache
Expand All @@ -53,17 +55,17 @@ class CSpace3Cache

char &e(const int &x, const int &y, const int &yaw)
{
assert(-center[0] <= x && x < size[0] - center[0]);
assert(-center[1] <= y && y < size[1] - center[1]);
assert(-center[2] <= yaw && yaw < size[2] - center[2]);
ROS_ASSERT(-center[0] <= x && x < size[0] - center[0]);
ROS_ASSERT(-center[1] <= y && y < size[1] - center[1]);
ROS_ASSERT(-center[2] <= yaw && yaw < size[2] - center[2]);

return c[(yaw * size[1] + (y + center[1])) * size[0] + x + center[0]];
}
const char &e(const int &x, const int &y, const int &yaw) const
{
assert(-center[0] <= x && x < size[0] - center[0]);
assert(-center[1] <= y && y < size[1] - center[1]);
assert(-center[2] <= yaw && yaw < size[2] - center[2]);
ROS_ASSERT(-center[0] <= x && x < size[0] - center[0]);
ROS_ASSERT(-center[1] <= y && y < size[1] - center[1]);
ROS_ASSERT(-center[2] <= yaw && yaw < size[2] - center[2]);

return c[(yaw * size[1] + (y + center[1])) * size[0] + x + center[0]];
}
Expand Down
5 changes: 3 additions & 2 deletions costmap_cspace/include/polygon.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#ifndef POLYGON_H
#define POLYGON_H

#include <ros/ros.h>
#include <vector>

namespace costmap_cspace
Expand All @@ -40,12 +41,12 @@ class Vec
float c[2];
float &operator[](const int &i)
{
assert(i < 2);
ROS_ASSERT(i < 2);
return c[i];
}
const float &operator[](const int &i) const
{
assert(i < 2);
ROS_ASSERT(i < 2);
return c[i];
}
Vec operator-(const Vec &a) const
Expand Down
Loading