diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 0333f03577b..c8ac2c65bf9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -201,6 +201,7 @@ class StaticLayer : public CostmapLayer bool track_unknown_space_; bool use_maximum_; unsigned char lethal_threshold_; + unsigned char inscribed_obstacle_cost_value_; unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 94aa69f87f8..009533e22c4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; using rcl_interfaces::msg::ParameterType; @@ -157,6 +158,7 @@ StaticLayer::getParameters() node->get_parameter("track_unknown_space", track_unknown_space_); node->get_parameter("use_maximum", use_maximum_); node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); + node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_); node->get_parameter("unknown_cost_value", unknown_cost_value_); node->get_parameter("trinary_costmap", trinary_costmap_); node->get_parameter("transform_tolerance", temp_tf_tol); @@ -267,6 +269,8 @@ StaticLayer::interpretValue(unsigned char value) return NO_INFORMATION; } else if (!track_unknown_space_ && value == unknown_cost_value_) { return FREE_SPACE; + } else if (value == inscribed_obstacle_cost_value_) { + return INSCRIBED_INFLATED_OBSTACLE; } else if (value >= lethal_threshold_) { return LETHAL_OBSTACLE; } else if (trinary_costmap_) { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index f43373d286b..577dff9fb21 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -131,6 +131,7 @@ void Costmap2DROS::init() declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); + declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); declare_parameter("subscribe_to_stamped_footprint", rclcpp::ParameterValue(false)); diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 658791beeb5..74a019a43b3 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -462,7 +462,7 @@ TEST_F(TestNode, testInflation) layers.updateMap(0, 0, 0); // printMap(*costmap); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20u); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28u); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 78u); /*/ Iterate over all id's and verify they are obstacles for(std::vector::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ @@ -480,7 +480,7 @@ TEST_F(TestNode, testInflation) // It and its 2 neighbors makes 3 obstacles ASSERT_EQ( countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) + - countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 51u); + countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 98u); // @todo Rewrite // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells @@ -494,7 +494,7 @@ TEST_F(TestNode, testInflation) // at <0, 1> ASSERT_EQ( countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) + - countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 54u); + countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 98u); // Add an obstacle at <1, 9>. This will inflate obstacles around it addObservation(olayer, 1, 9); diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index dc8a4f4b3a2..126cf7f99c7 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -268,7 +268,7 @@ TEST_F(TestNode, testDifferentInflationLayers) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); } TEST_F(TestNode, testDifferentInflationLayers2) { @@ -317,7 +317,7 @@ TEST_F(TestNode, testDifferentInflationLayers2) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); } TEST_F(TestNode, testResetting) { @@ -447,7 +447,7 @@ TEST_F(TestNode, testClearing) { nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); - ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 29); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 77); ASSERT_EQ(olayer_b->getCost(9, 9), nav2_costmap_2d::LETHAL_OBSTACLE); pclayer_a->clearArea(-1, -1, 10, 10, false);