Skip to content

Commit

Permalink
costmap_cspace: make static layers configurable (#108)
Browse files Browse the repository at this point in the history 
* Make static layers configurable

* Fix naming style of map_overlay_mode and add string to enum converter

* Add a static layer to the sample launch
  • Loading branch information
@at-wat
at-wat committed Jan 24, 2018
1 parent d5baf92 commit 08fb8b9
Show file tree
Hide file tree
Showing 5 changed files with 159 additions and 112 deletions.
9 changes: 3 additions & 6 deletions costmap_cspace/include/costmap_3d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,7 @@ class Costmap3d

costmap_base->setAngleResolution(ang_resolution_);

costmap_base->setOverlayMode(
Costmap3dLayerBase::map_overlay_mode::MAX);
costmap_base->setOverlayMode(MapOverlayMode::MAX);

costmaps_.resize(1);
costmaps_[0] = costmap_base;
Expand All @@ -75,8 +74,7 @@ class Costmap3d
}
template <typename T>
typename T::Ptr addLayer(
const Costmap3dLayerBase::map_overlay_mode overlay_mode =
Costmap3dLayerBase::map_overlay_mode::MAX)
const MapOverlayMode overlay_mode = MapOverlayMode::MAX)
{
typename T::Ptr costmap_overlay(new T);
costmap_overlay->setAngleResolution(ang_resolution_);
Expand All @@ -89,8 +87,7 @@ class Costmap3d
}
Costmap3dLayerBase::Ptr addLayer(
Costmap3dLayerBase::Ptr costmap_overlay,
const Costmap3dLayerBase::map_overlay_mode overlay_mode =
Costmap3dLayerBase::map_overlay_mode::MAX)
const MapOverlayMode overlay_mode = MapOverlayMode::MAX)
{
costmap_overlay->setAngleResolution(ang_resolution_);
costmap_overlay->setOverlayMode(overlay_mode);
Expand Down
149 changes: 75 additions & 74 deletions costmap_cspace/include/costmap_3d_layer/base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,89 +70,90 @@ class CSpace3DMsg : public CSpace3D
}
};

class Costmap3dLayerBase
enum MapOverlayMode
{
OVERWRITE,
MAX
};

class UpdatedRegion
{
public:
enum map_overlay_mode
int x_, y_, yaw_;
int width_, height_, angle_;
ros::Time stamp_;

UpdatedRegion()
: x_(0)
, y_(0)
, yaw_(0)
, width_(0)
, height_(0)
, angle_(0)
, stamp_(0)
{
OVERWRITE,
MAX
};
class UpdatedRegion
}
UpdatedRegion(
const int &x, const int &y, const int &yaw,
const int &width, const int &height, const int &angle,
const ros::Time &stamp)
: x_(x)
, y_(y)
, yaw_(yaw)
, width_(width)
, height_(height)
, angle_(angle)
, stamp_(stamp)
{
public:
int x_, y_, yaw_;
int width_, height_, angle_;
ros::Time stamp_;

UpdatedRegion()
: x_(0)
, y_(0)
, yaw_(0)
, width_(0)
, height_(0)
, angle_(0)
, stamp_(0)
{
}
UpdatedRegion(
const int &x, const int &y, const int &yaw,
const int &width, const int &height, const int &angle,
const ros::Time &stamp)
: x_(x)
, y_(y)
, yaw_(yaw)
, width_(width)
, height_(height)
, angle_(angle)
, stamp_(stamp)
{
}
void merge(const UpdatedRegion &region)
{
if (region.width_ == 0 || region.height_ == 0 || region.angle_ == 0)
return;
if (region.stamp_ > stamp_)
stamp_ = region.stamp_;
}
void merge(const UpdatedRegion &region)
{
if (region.width_ == 0 || region.height_ == 0 || region.angle_ == 0)
return;
if (region.stamp_ > stamp_)
stamp_ = region.stamp_;

if (width_ == 0 || height_ == 0 || angle_ == 0)
{
*this = region;
return;
}
int x2 = x_ + width_;
int y2 = y_ + height_;
int yaw2 = yaw_ + angle_;

const int mx2 = region.x_ + region.width_;
const int my2 = region.y_ + region.height_;
const int myaw2 = region.yaw_ + region.angle_;

if (region.x_ < x_)
x_ = region.x_;
if (region.y_ < y_)
y_ = region.y_;
if (region.yaw_ < yaw_)
yaw_ = region.yaw_;

if (x2 < mx2)
x2 = mx2;
if (y2 < my2)
y2 = my2;
if (yaw2 < myaw2)
yaw2 = myaw2;

width_ = x2 - x_;
height_ = y2 - y_;
angle_ = yaw2 - yaw_;
if (width_ == 0 || height_ == 0 || angle_ == 0)
{
*this = region;
return;
}
};
int x2 = x_ + width_;
int y2 = y_ + height_;
int yaw2 = yaw_ + angle_;

const int mx2 = region.x_ + region.width_;
const int my2 = region.y_ + region.height_;
const int myaw2 = region.yaw_ + region.angle_;

if (region.x_ < x_)
x_ = region.x_;
if (region.y_ < y_)
y_ = region.y_;
if (region.yaw_ < yaw_)
yaw_ = region.yaw_;

if (x2 < mx2)
x2 = mx2;
if (y2 < my2)
y2 = my2;
if (yaw2 < myaw2)
yaw2 = myaw2;

width_ = x2 - x_;
height_ = y2 - y_;
angle_ = yaw2 - yaw_;
}
};

class Costmap3dLayerBase
{
public:
using Ptr = std::shared_ptr<Costmap3dLayerBase>;

protected:
int ang_grid_;
map_overlay_mode overlay_mode_;
MapOverlayMode overlay_mode_;
bool root_;

CSpace3DMsg::Ptr map_;
Expand All @@ -165,7 +166,7 @@ class Costmap3dLayerBase
public:
Costmap3dLayerBase()
: ang_grid_(-1)
, overlay_mode_(map_overlay_mode::MAX)
, overlay_mode_(MapOverlayMode::MAX)
, root_(true)
, map_(new CSpace3DMsg)
, map_overlay_(new CSpace3DMsg)
Expand All @@ -181,7 +182,7 @@ class Costmap3dLayerBase
ang_grid_ = ang_resolution;
}
void setOverlayMode(
const map_overlay_mode overlay_mode)
const MapOverlayMode overlay_mode)
{
overlay_mode_ = overlay_mode;
}
Expand Down
89 changes: 66 additions & 23 deletions costmap_cspace/src/costmap_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,21 @@ class Costmap3DOFNode
pub_footprint_.publish(footprint);
}

static costmap_cspace::MapOverlayMode getMapOverlayModeFromString(
const std::string overlay_mode_str)
{
if (overlay_mode_str == "overwrite")
{
return costmap_cspace::MapOverlayMode::OVERWRITE;
}
else if (overlay_mode_str == "max")
{
return costmap_cspace::MapOverlayMode::MAX;
}
ROS_FATAL("Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
throw std::runtime_error("Unknown overlay_mode.");
};

public:
Costmap3DOFNode()
: nh_()
Expand Down Expand Up @@ -175,6 +190,50 @@ class Costmap3DOFNode
root_layer->setExpansion(linear_expand, linear_spread);
root_layer->setFootprint(footprint);

if (nhp_.hasParam("static_layers"))
{
XmlRpc::XmlRpcValue layers_xml;
nhp_.getParam("static_layers", layers_xml);

if (layers_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct || layers_xml.size() < 1)
{
ROS_FATAL("static_layers parameter must contain at least one layer config.");
throw std::runtime_error("layers parameter must contain at least one layer config.");
}
for (auto &layer_xml : layers_xml)
{
ROS_INFO("New static layer: %s", layer_xml.first.c_str());

costmap_cspace::MapOverlayMode overlay_mode(costmap_cspace::MapOverlayMode::MAX);
if (layer_xml.second["overlay_mode"].getType() == XmlRpc::XmlRpcValue::TypeString)
overlay_mode = getMapOverlayModeFromString(
layer_xml.second["overlay_mode"]);
else
ROS_WARN("overlay_mode of the static layer is not specified. Using MAX mode.");

std::string type;
if (layer_xml.second["type"].getType() == XmlRpc::XmlRpcValue::TypeString)
type = std::string(layer_xml.second["type"]);
else
{
ROS_FATAL("Layer type is not specified.");
throw std::runtime_error("Layer type is not specified.");
}

if (!layer_xml.second.hasMember("footprint"))
layer_xml.second["footprint"] = footprint_xml;

costmap_cspace::Costmap3dLayerBase::Ptr layer =
costmap_cspace::Costmap3dLayerClassLoader::loadClass(type);
costmap_->addLayer(layer, overlay_mode);
layer->loadConfig(layer_xml.second);

sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
layer_xml.first, 1,
boost::bind(&Costmap3DOFNode::cbMapOverlay, this, _1, layer)));
}
}

auto static_output_layer = costmap_->addLayer<costmap_cspace::Costmap3dLayerOutput>();
static_output_layer->setHandler(boost::bind(&Costmap3DOFNode::cbUpdateStatic, this, _1, _2));

Expand All @@ -195,41 +254,25 @@ class Costmap3DOFNode
for (auto &layer_xml : layers_xml)
{
ROS_INFO("New layer: %s", layer_xml.first.c_str());
costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode overlay_mode;

costmap_cspace::MapOverlayMode overlay_mode(costmap_cspace::MapOverlayMode::MAX);
if (layer_xml.second["overlay_mode"].getType() == XmlRpc::XmlRpcValue::TypeString)
{
std::string overlay_mode_str(layer_xml.second["overlay_mode"]);
if (overlay_mode_str.compare("overwrite") == 0)
overlay_mode = costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE;
else if (overlay_mode_str.compare("max") == 0)
overlay_mode = costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::MAX;
else
{
ROS_FATAL("Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
throw std::runtime_error("Unknown overlay_mode.");
}
}
overlay_mode = getMapOverlayModeFromString(
layer_xml.second["overlay_mode"]);
else
{
ROS_WARN("overlay_mode of the layer is not specified. Using MAX mode.");
overlay_mode = costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::MAX;
}

std::string type;
if (layer_xml.second["type"].getType() == XmlRpc::XmlRpcValue::TypeString)
{
type = std::string(layer_xml.second["type"]);
}
else
{
ROS_FATAL("Layer type is not specified.");
throw std::runtime_error("Layer type is not specified.");
}

if (!layer_xml.second.hasMember("footprint"))
{
layer_xml.second["footprint"] = footprint_xml;
}

costmap_cspace::Costmap3dLayerBase::Ptr layer =
costmap_cspace::Costmap3dLayerClassLoader::loadClass(type);
Expand All @@ -244,13 +287,13 @@ class Costmap3DOFNode
else
{
// Single layer mode for backward-compatibility
costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode overlay_mode;
costmap_cspace::MapOverlayMode overlay_mode;
std::string overlay_mode_str;
nhp_.param("overlay_mode", overlay_mode_str, std::string("max"));
if (overlay_mode_str.compare("overwrite") == 0)
overlay_mode = costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE;
overlay_mode = costmap_cspace::MapOverlayMode::OVERWRITE;
else if (overlay_mode_str.compare("max") == 0)
overlay_mode = costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::MAX;
overlay_mode = costmap_cspace::MapOverlayMode::MAX;
else
{
ROS_FATAL("Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
Expand Down
Loading

0 comments on commit 08fb8b9

Please sign in to comment.