Skip to content

Commit

Permalink
costmap_cspace: add cutoff_cost
Browse files Browse the repository at this point in the history
  • Loading branch information
@nhatao
nhatao committed Sep 11, 2023
1 parent f1720e0 commit e2b660d
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 5 deletions.
19 changes: 16 additions & 3 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 @@ -60,6 +60,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
geometry_msgs::PolygonStamped footprint_;
float linear_expand_;
float linear_spread_;
int cutoff_cost_;
Polygon footprint_p_;
bool keep_unknown_;

Expand All @@ -71,15 +72,18 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
Costmap3dLayerFootprint()
: linear_expand_(0.0)
, linear_spread_(0.0)
, cutoff_cost_(0)
, keep_unknown_(false)
, range_max_(0)
{
}
void loadConfig(XmlRpc::XmlRpcValue config)
{
const int cutoff_cost = config.hasMember("cuttoff_cost") ? static_cast<int>(config["cuttoff_cost"]) : 0;
setExpansion(
static_cast<double>(config["linear_expand"]),
static_cast<double>(config["linear_spread"]));
static_cast<double>(config["linear_spread"]),
cutoff_cost);
setFootprint(costmap_cspace::Polygon(config["footprint"]));
if (config.hasMember("keep_unknown"))
setKeepUnknown(config["keep_unknown"]);
Expand All @@ -90,15 +94,19 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
}
void setExpansion(
const float linear_expand,
const float linear_spread)
const float linear_spread,
const int cutoff_cost = 0)
{
linear_expand_ = linear_expand;
linear_spread_ = linear_spread;
cutoff_cost_ = cutoff_cost;

ROS_ASSERT(linear_expand >= 0.0);
ROS_ASSERT(std::isfinite(linear_expand));
ROS_ASSERT(linear_spread >= 0.0);
ROS_ASSERT(std::isfinite(linear_spread));
ROS_ASSERT(cutoff_cost_ >= 0);
ROS_ASSERT(cutoff_cost_ < 100);
}
void setFootprint(const Polygon footprint)
{
Expand Down Expand Up @@ -134,6 +142,7 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
std::ceil((footprint_radius_ + linear_expand_ + linear_spread_) / info.linear_resolution);
cs_template_.reset(range_max_, range_max_, info.angle);

const float eps = info.linear_resolution / 100.0;
// C-Space template
for (size_t yaw = 0; yaw < info.angle; yaw++)
{
Expand Down Expand Up @@ -161,7 +170,11 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
}
else if (d < linear_expand_ + linear_spread_)
{
cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * 100 / linear_spread_;
cs_template_.e(x, y, yaw) = 100 - (d - linear_expand_) * (100 - cutoff_cost_) / linear_spread_;
}
else if (std::abs(linear_expand_ + linear_spread_ - d) < eps)
{
cs_template_.e(x, y, yaw) = cutoff_cost_;
}
else
{
Expand Down
4 changes: 3 additions & 1 deletion costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,11 @@ class Costmap3dLayerPlain : public Costmap3dLayerFootprint
}
void loadConfig(XmlRpc::XmlRpcValue config)
{
const int cutoff_cost = config.hasMember("cuttoff_cost") ? static_cast<int>(config["cuttoff_cost"]) : 0;
setExpansion(
static_cast<double>(config["linear_expand"]),
static_cast<double>(config["linear_spread"]));
static_cast<double>(config["linear_spread"]),
cutoff_cost);
}
};
} // namespace costmap_cspace
Expand Down
4 changes: 3 additions & 1 deletion costmap_cspace/src/costmap_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,9 @@ class Costmap3DOFNode
float linear_spread;
pnh_.param("linear_expand", linear_expand, 0.2f);
pnh_.param("linear_spread", linear_spread, 0.5f);
root_layer->setExpansion(linear_expand, linear_spread);
int cutoff_cost;
pnh_.param("cutoff_cost", cutoff_cost, 0);
root_layer->setExpansion(linear_expand, linear_spread, cutoff_cost);
root_layer->setFootprint(footprint);

if (pnh_.hasParam("static_layers"))
Expand Down
89 changes: 89 additions & 0 deletions costmap_cspace/test/src/test_costmap_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1021,6 +1021,95 @@ TEST(Costmap3dLayerFootprint, PlainOnFootprint)
}
}

TEST(Costmap3dLayerOutput, CutoffCost)
{
// clang-format off
const std::vector<int8_t> input_base(49, 0);
const std::vector<int8_t> input_layer =
{
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
};
const std::vector<float> resolutions =
{
1.0,
0.1,
};
const std::vector<int> cutoffs =
{
0,
50,
};
const std::vector<std::vector<int8_t>> expected_results_cutoffs =
{
{
0, 0, 0, 0, 0, 0, 0,
0, 5, 25, 33, 25, 5, 0,
0, 25, 52, 66, 52, 25, 0,
0, 33, 66, 100, 66, 33, 0,
0, 25, 52, 66, 52, 25, 0,
0, 5, 25, 33, 25, 5, 0,
0, 0, 0, 0, 0, 0, 0,
}, // NOLINT(whitespace/braces)
{
0, 0, 0, 50, 0, 0, 0,
0, 52, 62, 66, 62, 52, 0,
0, 62, 76, 83, 76, 62, 0,
50, 66, 83, 100, 83, 66, 50,
0, 62, 76, 83, 76, 62, 0,
0, 52, 62, 66, 62, 52, 0,
0, 0, 0, 50, 0, 0, 0,
}
};
// clang-format on

for (const float resolution : resolutions)
{
for (size_t i = 0; i < cutoffs.size(); ++i)
{
std::ostringstream oss_test_name;
oss_test_name << "resolution: " << resolution << " cutoff: " << cutoffs[i];
SCOPED_TRACE(oss_test_name.str());

nav_msgs::OccupancyGrid::Ptr map_base(new nav_msgs::OccupancyGrid);
map_base->info.width = 7;
map_base->info.height = 7;
map_base->info.resolution = resolution;
map_base->info.origin.orientation.w = 1.0;
map_base->data = input_base;

nav_msgs::OccupancyGrid::Ptr map_layer(new nav_msgs::OccupancyGrid);
map_layer->info.width = 7;
map_layer->info.height = 7;
map_layer->info.resolution = resolution;
map_layer->info.origin.orientation.w = 1.0;
map_layer->data = input_layer;

costmap_cspace::Costmap3d cms(1);
auto cm = cms.addRootLayer<costmap_cspace::Costmap3dLayerPlain>();
auto cm_over = cms.addLayer<costmap_cspace::Costmap3dLayerPlain>(
costmap_cspace::MapOverlayMode::OVERWRITE);
cm_over->setExpansion(0.0, resolution * 3.0, cutoffs[i]);
cm->setBaseMap(map_base);
cm_over->processMapOverlay(map_layer, true);
const costmap_cspace::CSpace3DMsg::Ptr overlay_map = cm_over->getMapOverlay();

const auto& expected_result = expected_results_cutoffs[i];
ASSERT_EQ(expected_result.size(), overlay_map->data.size());
for (size_t i = 0; i < expected_result.size(); ++i)
{
EXPECT_EQ(expected_result[i], overlay_map->data[i])
<< " Different at: (" << i % map_layer->info.width << "," << i / 5 << ")";
}
}
}
}

int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down

0 comments on commit e2b660d

Please sign in to comment.