From 600e7bb31a843c92a25f9d9230ba1a7f7190fdc0 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Wed, 13 Sep 2023 08:36:12 +0900 Subject: [PATCH] costmap_cspace: add linear_spread_min_cost parameter (#719) --- costmap_cspace/README.md | 9 ++ .../costmap_3d_layer/footprint.h | 20 ++++- .../costmap_cspace/costmap_3d_layer/plain.h | 5 +- costmap_cspace/src/costmap_3d.cpp | 4 +- costmap_cspace/test/src/test_costmap_3d.cpp | 89 +++++++++++++++++++ 5 files changed, 122 insertions(+), 5 deletions(-) diff --git a/costmap_cspace/README.md b/costmap_cspace/README.md index 23fd601b2..9437979f3 100644 --- a/costmap_cspace/README.md +++ b/costmap_cspace/README.md @@ -26,6 +26,7 @@ costmap_3d node converts 2-D (x, y) OccupancyGrid to 2-D/3-DOF (x, y, yaw) confi * "ang_resolution" (int, default: 16) * "linear_expand" (double, default: 0.2f) * "linear_spread" (double, default: 0.5f) +* "linear_spread_min_cost" (int, default: 0) * "unknown_cost" (int, default: 0) * "overlay_mode" (string, default: std::string("")) * "footprint" (?, default: footprint_xml) @@ -49,6 +50,7 @@ costmap_3d node converts 2-D (x, y) OccupancyGrid to 2-D/3-DOF (x, y, yaw) confi * "ang_resolution" (int, default: 16): for root layer * "linear_expand" (double, default: 0.2f): for root layer * "linear_spread" (double, default: 0.5f): for root layer +* "linear_spread_min_cost" (int, default: 0) * "footprint" (?, default: footprint_xml): for root layer * "static_layers": array of layer configurations * "layers": array of layer configurations @@ -62,10 +64,12 @@ Available layer types and parameters are: - **Costmap3dLayerFootprint**: Configuration space costmap layer according to the given footprint. - "linear_expand" (double) - "linear_spread" (double) + - "linear_spread_min_cost" (int, default: 0) - "footprint" (?, default: root layer's footprint) - **Costmap3dLayerPlain**: Costmap layer without considering footpring. - "linear_expand" (double) - "linear_spread" (double) + - "linear_spread_min_cost" (int, default: 0) - **Costmap3dLayerOutput**: Output generated costmap at this point. In most case, this is placed at the last layer. - **Costmap3dLayerStopPropagation**: Stop propagating parent layer's cost to the child. This can be used at the beginning of layer to ignore changes in static layers. - **Costmap3dLayerUnknownHandle**: Set unknown cell's cost. @@ -73,6 +77,11 @@ Available layer types and parameters are: See [example parameters](https://github.com/at-wat/neonavigation/blob/master/neonavigation_launch/config/navigate.yaml). +The diagram below shows how "linear_expand", "linear_spread" and "linear_spread_min_cost" work. +"linear_spread_min_cost" can make the costs of grids near obstacles higher to avoid other costs such as preferences overriding the costs of these grids. + +![Screenshot from 2023-09-12 17-19-52](https://github.com/at-wat/neonavigation/assets/8833040/58001b49-b603-47e9-92e7-0517f0aaf8ba) + ---- ## laserscan_to_map 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 ee0621bea..9294688dd 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 linear_spread_min_cost_; Polygon footprint_p_; bool keep_unknown_; @@ -71,15 +72,19 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase Costmap3dLayerFootprint() : linear_expand_(0.0) , linear_spread_(0.0) + , linear_spread_min_cost_(0) , keep_unknown_(false) , range_max_(0) { } void loadConfig(XmlRpc::XmlRpcValue config) { + const int linear_spread_min_cost = + config.hasMember("linear_spread_min_cost") ? static_cast(config["linear_spread_min_cost"]) : 0; setExpansion( static_cast(config["linear_expand"]), - static_cast(config["linear_spread"])); + static_cast(config["linear_spread"]), + linear_spread_min_cost); setFootprint(costmap_cspace::Polygon(config["footprint"])); if (config.hasMember("keep_unknown")) setKeepUnknown(config["keep_unknown"]); @@ -90,15 +95,19 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase } void setExpansion( const float linear_expand, - const float linear_spread) + const float linear_spread, + const int linear_spread_min_cost = 0) { linear_expand_ = linear_expand; linear_spread_ = linear_spread; + linear_spread_min_cost_ = linear_spread_min_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(linear_spread_min_cost_ >= 0); + ROS_ASSERT(linear_spread_min_cost_ < 100); } void setFootprint(const Polygon footprint) { @@ -134,6 +143,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 +171,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 - linear_spread_min_cost_) / linear_spread_; + } + else if (std::abs(linear_expand_ + linear_spread_ - d) < eps) + { + cs_template_.e(x, y, yaw) = linear_spread_min_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 90747cd83..38174b33e 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,12 @@ class Costmap3dLayerPlain : public Costmap3dLayerFootprint } void loadConfig(XmlRpc::XmlRpcValue config) { + const int linear_spread_min_cost = + config.hasMember("linear_spread_min_cost") ? static_cast(config["linear_spread_min_cost"]) : 0; setExpansion( static_cast(config["linear_expand"]), - static_cast(config["linear_spread"])); + static_cast(config["linear_spread"]), + linear_spread_min_cost); } }; } // namespace costmap_cspace diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp index cd81fe910..f58435000 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 linear_spread_min_cost; + pnh_.param("linear_spread_min_cost", linear_spread_min_cost, 0); + root_layer->setExpansion(linear_expand, linear_spread, linear_spread_min_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 24ae13c00..3f92b1430 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, LinearSpreadMinCost) +{ + // 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);