diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h index ee0621be..b37091d6 100644 --- a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h +++ b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/footprint.h @@ -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_; @@ -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(config["cuttoff_cost"]) : 0; setExpansion( static_cast(config["linear_expand"]), - static_cast(config["linear_spread"])); + static_cast(config["linear_spread"]), + cutoff_cost); setFootprint(costmap_cspace::Polygon(config["footprint"])); if (config.hasMember("keep_unknown")) setKeepUnknown(config["keep_unknown"]); @@ -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) { @@ -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++) { @@ -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 { diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h index 90747cd8..475ab453 100644 --- a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h +++ b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/plain.h @@ -59,9 +59,11 @@ class Costmap3dLayerPlain : public Costmap3dLayerFootprint } void loadConfig(XmlRpc::XmlRpcValue config) { + const int cutoff_cost = config.hasMember("cuttoff_cost") ? static_cast(config["cuttoff_cost"]) : 0; setExpansion( static_cast(config["linear_expand"]), - static_cast(config["linear_spread"])); + static_cast(config["linear_spread"]), + cutoff_cost); } }; } // namespace costmap_cspace diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp index cd81fe91..d1fe5c4d 100644 --- a/costmap_cspace/src/costmap_3d.cpp +++ b/costmap_cspace/src/costmap_3d.cpp @@ -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")) diff --git a/costmap_cspace/test/src/test_costmap_3d.cpp b/costmap_cspace/test/src/test_costmap_3d.cpp index 24ae13c0..2e15d414 100644 --- a/costmap_cspace/test/src/test_costmap_3d.cpp +++ b/costmap_cspace/test/src/test_costmap_3d.cpp @@ -1021,6 +1021,95 @@ TEST(Costmap3dLayerFootprint, PlainOnFootprint) } } +TEST(Costmap3dLayerOutput, CutoffCost) +{ + // clang-format off + const std::vector input_base(49, 0); + const std::vector 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 resolutions = + { + 1.0, + 0.1, + }; + const std::vector cutoffs = + { + 0, + 50, + }; + const std::vector> 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(); + auto cm_over = cms.addLayer( + 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);