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 13f20ef3b07..966f7eeecd5 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 0fa21ad1a50..5b7e05fb30e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -133,6 +133,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 590d4a9208f..dbf48d37cf0 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -181,6 +181,7 @@ void TestNode::initNode(std::vector parameters) node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index b55436a9fc7..35b0169a0b7 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -101,6 +101,7 @@ class TestNode : public ::testing::Test node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); @@ -580,6 +581,7 @@ class TestNodeWithoutUnknownOverwrite : public ::testing::Test node_ = std::make_shared("obstacle_test_node"); node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(true)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index 5302ed2e70d..a1920df53e3 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -82,6 +82,7 @@ class TestNode : public ::testing::Test node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp index 5d48dfabac1..e9b582e4a15 100644 --- a/nav2_costmap_2d/test/integration/range_tests.cpp +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -105,6 +105,7 @@ class TestNode : public ::testing::Test node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index b4ee7b3147e..8205c9f8781 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -93,6 +93,7 @@ class TestCollisionChecker : public nav2::LifecycleNode declare_parameter("track_unknown_space", rclcpp::ParameterValue(true)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); diff --git a/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp index 031b0551e88..f8d1178a68d 100644 --- a/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp @@ -83,6 +83,7 @@ class ObstacleLayerTest : public ::testing::Test node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("inscribed_obstacle_cost_value", rclcpp::ParameterValue(99)); node_->declare_parameter( "unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); diff --git a/nav2_planner/test/test_plan_through_poses.cpp b/nav2_planner/test/test_plan_through_poses.cpp index 0e5ad170c44..f0aa854c07c 100644 --- a/nav2_planner/test/test_plan_through_poses.cpp +++ b/nav2_planner/test/test_plan_through_poses.cpp @@ -170,6 +170,8 @@ void Tester::setParameters() costmap->declare_parameter("static_layer.plugin", rclcpp::ParameterValue(COSTMAP_LAYER_NAME)); costmap->declare_parameter("static_layer.lethal_cost_threshold", rclcpp::ParameterValue(100)); + costmap->declare_parameter("static_layer.inscribed_obstacle_cost_value", + rclcpp::ParameterValue(99)); costmap->declare_parameter( "static_layer.map_subscribe_transient_local", rclcpp::ParameterValue(true));