costmap_cspace: add linear_spread_min_cost parameter

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,17 +64,24 @@ 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.
   - "unknown_cost" (int)
 
 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
 

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<int>(config["linear_spread_min_cost"]) : 0;
     setExpansion(
         static_cast<double>(config["linear_expand"]),
-        static_cast<double>(config["linear_spread"]));
+        static_cast<double>(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
             {

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<int>(config["linear_spread_min_cost"]) : 0;
     setExpansion(
         static_cast<double>(config["linear_expand"]),
-        static_cast<double>(config["linear_spread"]));
+        static_cast<double>(config["linear_spread"]),
+        linear_spread_min_cost);
   }
 };
 }  // namespace costmap_cspace

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"))

costmap_cspace/test/src/test_costmap_3d.cpp

@@ -1021,6 +1021,95 @@ TEST(Costmap3dLayerFootprint, PlainOnFootprint)
   }
 }
 
+TEST(Costmap3dLayerOutput, LinearSpreadMinCost)
+{
+  // 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);