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: make costmap layer structure configurable #106

Merged
merged 15 commits into from
Jan 24, 2018
Merged

costmap_cspace: make costmap layer structure configurable #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
4b31d59
costmap_cspace: make costmap layer structure configurable
at-wat Jan 23, 2018
ebbee61
Don't clear region after publish
at-wat Jan 23, 2018
47eabf9
Fix parameter handling
at-wat Jan 23, 2018
cb0a7c1
Enable multi layer mode in sample launch
at-wat Jan 23, 2018
9006e1e
Select layer type by parameter
at-wat Jan 23, 2018
9633ced
Add class loader
at-wat Jan 24, 2018
1153fac
Move layer classes into include/costmap_3d_layer/
at-wat Jan 24, 2018
c1e18d1
Use output layer to publish static cspace costmap
at-wat Jan 24, 2018
f9a85fa
Separate class_loader header
at-wat Jan 24, 2018
759614d
Remove layer type specific variables from base class
at-wat Jan 24, 2018
97c17df
Rename generateCSpaceTemplate
at-wat Jan 24, 2018
f9efa47
Remove cspace template from base class
at-wat Jan 24, 2018
0d8e4eb
Merge branch 'master' into costmap_cspace/configurable-costmap-layers
at-wat Jan 24, 2018
6f2e800
Fix single layer mode
at-wat Jan 24, 2018
f49c4bb
Fix indent of sample launch file
at-wat Jan 24, 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
2 changes: 1 addition & 1 deletion costmap_cspace/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ endif()

include_directories(${catkin_INCLUDE_DIRS} include)

add_executable(costmap_3d src/costmap_3d.cpp)
add_executable(costmap_3d src/costmap_3d.cpp src/costmap_3d_layers.cpp)
target_link_libraries(costmap_3d ${catkin_LIBRARIES})
add_dependencies(costmap_3d costmap_cspace_generate_messages)

Expand Down
48 changes: 20 additions & 28 deletions costmap_cspace/include/costmap_3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,11 @@

#include <ros/ros.h>

#include <costmap_3d_layer_footprint.h>
#include <costmap_3d_layer_plain.h>
#include <costmap_3d_layer_output.h>
#include <costmap_3d_layer/footprint.h>
#include <costmap_3d_layer/plain.h>
#include <costmap_3d_layer/output.h>

#include <costmap_3d_layer/class_loader.h>

#include <vector>

Expand All @@ -45,45 +47,27 @@ class Costmap3d
protected:
std::vector<Costmap3dLayerBase::Ptr> costmaps_;
int ang_resolution_;
float linear_expand_;
float linear_spread_;
Polygon footprint_;

public:
using Ptr = std::shared_ptr<Costmap3d>;

Costmap3d(
const int ang_resolution,
const float linear_expand,
const float linear_spread,
const Polygon footprint = Polygon())
explicit Costmap3d(const int ang_resolution)
{
ang_resolution_ = ang_resolution;
linear_expand_ = linear_expand;
linear_spread_ = linear_spread;
footprint_ = footprint;

ROS_ASSERT(ang_resolution_ > 0);
ROS_ASSERT(linear_expand_ >= 0);
ROS_ASSERT(std::isfinite(linear_expand_));
ROS_ASSERT(linear_spread_ >= 0);
ROS_ASSERT(std::isfinite(linear_spread_));
}
template <typename T>
typename T::Ptr addRootLayer()
{
typename T::Ptr
costmap_base(new T);

costmap_base->setCSpaceConfig(
ang_resolution_,
linear_expand_,
linear_spread_);
costmap_base->setAngleResolution(ang_resolution_);

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

costmap_base->setFootprint(footprint_);
costmaps_.resize(1);
costmaps_[0] = costmap_base;

Expand All @@ -95,13 +79,21 @@ class Costmap3d
Costmap3dLayerBase::map_overlay_mode::MAX)
{
typename T::Ptr costmap_overlay(new T);
costmap_overlay->setCSpaceConfig(
ang_resolution_,
linear_expand_,
linear_spread_);
costmap_overlay->setAngleResolution(ang_resolution_);
costmap_overlay->setOverlayMode(overlay_mode);

costmap_overlay->setFootprint(footprint_);
costmaps_.back()->setChild(costmap_overlay);
costmaps_.push_back(costmap_overlay);

return costmap_overlay;
}
Costmap3dLayerBase::Ptr addLayer(
Costmap3dLayerBase::Ptr costmap_overlay,
const Costmap3dLayerBase::map_overlay_mode overlay_mode =
Costmap3dLayerBase::map_overlay_mode::MAX)
{
costmap_overlay->setAngleResolution(ang_resolution_);
costmap_overlay->setOverlayMode(overlay_mode);

costmaps_.back()->setChild(costmap_overlay);
costmaps_.push_back(costmap_overlay);
Expand Down
140 changes: 21 additions & 119 deletions ...ap_cspace/include/costmap_3d_layer_base.h → ...ap_cspace/include/costmap_3d_layer/base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,8 @@
#include <costmap_cspace/CSpace3D.h>
#include <costmap_cspace/CSpace3DUpdate.h>

#include <cspace3_cache.h>
#include <polygon.h>
#include <string>
#include <map>

namespace costmap_cspace
{
Expand Down Expand Up @@ -69,6 +69,7 @@ class CSpace3DMsg : public CSpace3D
return data[addr];
}
};

class Costmap3dLayerBase
{
public:
Expand Down Expand Up @@ -151,14 +152,9 @@ class Costmap3dLayerBase

protected:
int ang_grid_;
float linear_expand_;
float linear_spread_;
map_overlay_mode overlay_mode_;
bool root_;

CSpace3Cache cs_template_;
int range_max_;

CSpace3DMsg::Ptr map_;
CSpace3DMsg::Ptr map_overlay_;

Expand All @@ -169,44 +165,32 @@ class Costmap3dLayerBase
public:
Costmap3dLayerBase()
: ang_grid_(-1)
, linear_expand_(0.0)
, linear_spread_(0.0)
, overlay_mode_(map_overlay_mode::MAX)
, root_(true)
, range_max_(0)
, map_(new CSpace3DMsg)
, map_overlay_(new CSpace3DMsg)
{
}
void setCSpaceConfig(
const int ang_resolution,
const float linear_expand,
const float linear_spread)

virtual void loadConfig(XmlRpc::XmlRpcValue config) = 0;
virtual void setMapMetaData(const MapMetaData3D &info) = 0;

void setAngleResolution(
const int ang_resolution)
{
ang_grid_ = ang_resolution;
linear_expand_ = linear_expand;
linear_spread_ = linear_spread;

ROS_ASSERT(linear_expand >= 0.0);
ROS_ASSERT(std::isfinite(linear_expand));
ROS_ASSERT(linear_spread >= 0.0);
ROS_ASSERT(std::isfinite(linear_spread));
}
void setOverlayMode(
const map_overlay_mode overlay_mode)
{
overlay_mode_ = overlay_mode;
}
void setFootprint(const Polygon footprint)
{
}
void setChild(Costmap3dLayerBase::Ptr child)
{
child_ = child;
child_->setMap(getMapOverlay());
child_->root_ = false;
}
virtual void generateCSpaceTemplate(const MapMetaData3D &info) = 0;
void setBaseMap(const nav_msgs::OccupancyGrid &base_map)
{
ROS_ASSERT(root_);
Expand All @@ -223,7 +207,7 @@ class Costmap3dLayerBase
map_->info.origin = base_map.info.origin;
map_->data.resize(xy_size * map_->info.angle);

generateCSpaceTemplate(map_->info);
setMapMetaData(map_->info);

for (size_t yaw = 0; yaw < map_->info.angle; yaw++)
{
Expand All @@ -232,7 +216,7 @@ class Costmap3dLayerBase
map_->data[i + yaw * xy_size] = 0;
}
}
gemerateCSpace(map_, base_map);
updateCSpace(base_map);
for (size_t yaw = 0; yaw < map_->info.angle; yaw++)
{
for (unsigned int i = 0; i < xy_size; i++)
Expand All @@ -246,6 +230,11 @@ class Costmap3dLayerBase
*map_overlay_ = *map_;
if (child_)
child_->setBaseMapChain();

updateChainEntry(
UpdatedRegion(
0, 0, 0, map_->info.width, map_->info.height, map_->info.angle,
base_map.header.stamp));
}
void processMapOverlay(const nav_msgs::OccupancyGrid &msg)
{
Expand Down Expand Up @@ -273,126 +262,39 @@ class Costmap3dLayerBase
{
return map_overlay_;
}
CSpace3Cache &getTemplate()
{
return cs_template_;
}
int getRangeMax() const
{
return range_max_;
}
int getAngularGrid() const
{
return ang_grid_;
}

protected:
virtual bool updateChain() = 0;
virtual void updateCSpace(const nav_msgs::OccupancyGrid &map) = 0;

bool updateChainEntry(const UpdatedRegion &region)
{
region_.merge(region);

*map_overlay_ = *map_;
if (map_updated_.info.width > 0 && map_updated_.info.height > 0)
gemerateCSpace(map_overlay_, map_updated_, true);
updateCSpace(map_updated_);

if (updateChain())
{
region_ = UpdatedRegion();
return true;
}

if (child_)
{
const bool clear = child_->updateChainEntry(region_);
if (clear)
region_ = UpdatedRegion();
return clear;
return child_->updateChainEntry(region_);
}
return false;
}
void setBaseMapChain()
{
generateCSpaceTemplate(map_->info);
setMapMetaData(map_->info);
*map_overlay_ = *map_;
if (child_)
child_->setBaseMapChain();
}
void gemerateCSpace(CSpace3DMsg::Ptr map, const nav_msgs::OccupancyGrid &msg, bool overlay = false)
{
ROS_ASSERT(ang_grid_ > 0);
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);
// Clear travelable area in OVERWRITE mode
if (overlay_mode_ == OVERWRITE && overlay)
{
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
{
for (unsigned int i = 0; i < msg.data.size(); i++)
{
const auto &val = msg.data[i];
if (val < 0)
continue;

const int x = lroundf((i % msg.info.width) * msg.info.resolution / map->info.linear_resolution);
if (x < range_max_ || static_cast<int>(msg.info.width) - range_max_ <= x)
continue;
const int y = lroundf((i / msg.info.width) * msg.info.resolution / map->info.linear_resolution);
if (y < range_max_ || static_cast<int>(msg.info.height) - range_max_ <= y)
continue;

const int res_up = ceilf(msg.info.resolution / map->info.linear_resolution);
for (int yp = 0; yp < res_up; yp++)
{
for (int xp = 0; xp < res_up; xp++)
{
const int x2 = x + ox + xp;
const int y2 = y + oy + yp;
if (static_cast<unsigned int>(x2) >= map->info.width ||
static_cast<unsigned int>(y2) >= map->info.height)
continue;

auto &m = map->getCost(x2, y2, yaw);
m = 0;
}
}
}
}
}
// Get max
for (size_t yaw = 0; yaw < map->info.angle; yaw++)
{
for (unsigned int i = 0; i < msg.data.size(); i++)
{
const int gx = lroundf((i % msg.info.width) * msg.info.resolution / map->info.linear_resolution) + ox;
const int gy = lroundf((i / msg.info.width) * msg.info.resolution / map->info.linear_resolution) + oy;
if ((unsigned int)gx >= map->info.width ||
(unsigned int)gy >= map->info.height)
continue;

auto val = msg.data[i];
if (val <= 0)
continue;

for (int y = -range_max_; y <= range_max_; y++)
{
for (int x = -range_max_; x <= range_max_; x++)
{
const int x2 = gx + x;
const int y2 = gy + y;
if (static_cast<unsigned int>(x2) >= map->info.width ||
static_cast<unsigned int>(y2) >= map->info.height)
continue;

auto &m = map->getCost(x2, y2, yaw);
const auto c = cs_template_.e(x, y, yaw) * val / 100;
if (m < c)
m = c;
}
}
}
}
}
};
} // namespace costmap_cspace

Expand Down
Loading