diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 7e4aa7835bf..553cad825dc 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -363,15 +363,53 @@ StaticLayer::updateBounds( useExtraBounds(min_x, min_y, max_x, max_y); - double wx, wy; + // Might even be in a different frame + if (map_frame_ == global_frame_) { + double wx, wy; + + mapToWorld(x_, y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + + mapToWorld(x_ + width_, y_ + height_, wx, wy); + *max_x = std::max(wx, *max_x); + *max_y = std::max(wy, *max_y); + } else { + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_->lookupTransform( + global_frame_, map_frame_, tf2::TimePointZero, + transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); + return; + } + tf2::Transform tf2_transform; + tf2::fromMsg(transform.transform, tf2_transform); + + double wx, wy; + mapToWorld(x_, y_, wx, wy); + tf2::Vector3 down_left(wx, wy, 0); + + mapToWorld(x_ + width_, y_, wx, wy); + tf2::Vector3 down_right(wx, wy, 0); + + mapToWorld(x_, y_ + height_, wx, wy); + tf2::Vector3 up_left(wx, wy, 0); - mapToWorld(x_, y_, wx, wy); - *min_x = std::min(wx, *min_x); - *min_y = std::min(wy, *min_y); + mapToWorld(x_ + width_, y_ + height_, wx, wy); + tf2::Vector3 up_right(wx, wy, 0); - mapToWorld(x_ + width_, y_ + height_, wx, wy); - *max_x = std::max(wx, *max_x); - *max_y = std::max(wy, *max_y); + down_left = tf2_transform * down_left; + down_right = tf2_transform * down_right; + up_left = tf2_transform * up_left; + up_right = tf2_transform * up_right; + + *min_x = std::min({*min_x, down_left.x(), down_right.x(), up_left.x(), up_right.x()}); + *min_y = std::min({*min_y, down_left.y(), down_right.y(), up_left.y(), up_right.y()}); + *max_x = std::max({*max_x, down_left.x(), down_right.x(), up_left.x(), up_right.x()}); + *max_y = std::max({*max_y, down_left.y(), down_right.y(), up_left.y(), up_right.y()}); + } has_updated_data_ = false; diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 3cbf22336e8..3fa6c0e0039 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -201,7 +201,7 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove) { initNode(4.1); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 @@ -233,7 +233,7 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) { initNode(4.1); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 @@ -269,7 +269,7 @@ TEST_F(TestNode, testInflationInUnknown) node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(9, 9, 1, 0, 0); // Footprint with inscribed radius = 2.1 @@ -305,7 +305,7 @@ TEST_F(TestNode, testInflationAroundUnknown) node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 @@ -330,7 +330,7 @@ TEST_F(TestNode, testCostFunctionCorrectness) { initNode(10.5); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(100, 100, 1, 0, 0); // Footprint with inscribed radius = 5.0 @@ -405,7 +405,7 @@ TEST_F(TestNode, testInflationOrderCorrectness) const double inflation_radius = 4.1; initNode(inflation_radius); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 @@ -438,7 +438,7 @@ TEST_F(TestNode, testInflation) { initNode(1); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 @@ -516,7 +516,7 @@ TEST_F(TestNode, testInflation2) { initNode(1); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 @@ -554,7 +554,7 @@ TEST_F(TestNode, testInflation3) { initNode(3); tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); // 1 2 3 diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 56d422e678c..2676cccf4d4 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -148,7 +148,7 @@ class TestNode : public ::testing::Test TEST_F(TestNode, testRaytracing) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); addStaticLayer(layers, tf, node_); auto olayer = addObstacleLayer(layers, tf, node_); @@ -170,7 +170,7 @@ TEST_F(TestNode, testRaytracing) { */ TEST_F(TestNode, testRaytracing2) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); addStaticLayer(layers, tf, node_); auto olayer = addObstacleLayer(layers, tf, node_); @@ -227,7 +227,7 @@ TEST_F(TestNode, testWaveInterference) { tf2_ros::Buffer tf(node_->get_clock()); node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); // Start with an empty map, no rolling window, tracking unknown - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(10, 10, 1, 0, 0); auto olayer = addObstacleLayer(layers, tf, node_); @@ -255,7 +255,7 @@ TEST_F(TestNode, testWaveInterference) { TEST_F(TestNode, testZThreshold) { tf2_ros::Buffer tf(node_->get_clock()); // Start with an empty map - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(10, 10, 1, 0, 0); auto olayer = addObstacleLayer(layers, tf, node_); @@ -275,7 +275,7 @@ TEST_F(TestNode, testZThreshold) { */ TEST_F(TestNode, testDynamicObstacles) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); addStaticLayer(layers, tf, node_); auto olayer = addObstacleLayer(layers, tf, node_); @@ -300,7 +300,7 @@ TEST_F(TestNode, testDynamicObstacles) { */ TEST_F(TestNode, testMultipleAdditions) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); addStaticLayer(layers, tf, node_); auto olayer = addObstacleLayer(layers, tf, node_); @@ -319,7 +319,7 @@ TEST_F(TestNode, testMultipleAdditions) { */ TEST_F(TestNode, testRepeatedResets) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); std::shared_ptr slayer = nullptr; addStaticLayer(layers, tf, node_, slayer); @@ -370,7 +370,7 @@ TEST_F(TestNode, testRepeatedResets) { TEST_F(TestNode, testRaytracing) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); std::shared_ptr slayer = nullptr; @@ -555,7 +555,7 @@ TEST_F(TestNode, testMaxCombinationMethod) { tf2_ros::Buffer tf(node_->get_clock()); // Create a costmap with full unknown space - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(10, 10, 1, 0, 0); std::shared_ptr olayer = nullptr; @@ -600,7 +600,7 @@ TEST_F(TestNodeWithoutUnknownOverwrite, testMaxWithoutUnknownOverwriteCombinatio tf2_ros::Buffer tf(node_->get_clock()); // Create a costmap with full unknown space - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(10, 10, 1, 0, 0); std::shared_ptr olayer = nullptr; diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index 06e15ea6e6a..4dbe4bcdf5e 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -158,7 +158,7 @@ class TestNode : public ::testing::Test TEST_F(TestNode, testObstacleLayers) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); std::shared_ptr pclayer_a = nullptr; @@ -191,7 +191,7 @@ TEST_F(TestNode, testObstacleAndStaticLayers) { node_->declare_parameter("pclayer_a.static.map_topic", rclcpp::ParameterValue(std::string("map"))); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -232,7 +232,7 @@ TEST_F(TestNode, testDifferentInflationLayers) { node_->declare_parameter("pclayer_b.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -281,7 +281,7 @@ TEST_F(TestNode, testDifferentInflationLayers2) { node_->declare_parameter("pclayer_a.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -336,7 +336,7 @@ TEST_F(TestNode, testResetting) { node_->declare_parameter("pclayer_b.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -407,7 +407,7 @@ TEST_F(TestNode, testClearing) { node_->declare_parameter("pclayer_b.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -473,7 +473,7 @@ TEST_F(TestNode, testOverwriteCombinationMethods) { node_->declare_parameter("pclayer_b.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layers.resizeMap(10, 10, 1, 0, 0); @@ -534,7 +534,7 @@ TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) { node_->declare_parameter("pclayer_b.inflation.inflation_radius", rclcpp::ParameterValue(1.0)); - nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + nav2_costmap_2d::LayeredCostmap layers("map", false, true); layers.resizeMap(10, 10, 1, 0, 0); @@ -580,7 +580,7 @@ TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) { TEST_F(TestNode, testClearable) { tf2_ros::Buffer tf(node_->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); std::shared_ptr pclayer_a = nullptr; addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a"); diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index 4106936cef1..2bd846a1168 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -34,7 +34,7 @@ TEST(DeclareParameter, useValidParameter) nav2_util::LifecycleNode::SharedPtr node = std::make_shared("test_node"); tf2_ros::Buffer tf(node->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layer.initialize(&layers, "test_layer", &tf, node, nullptr); @@ -53,7 +53,7 @@ TEST(DeclareParameter, useInvalidParameter) nav2_util::LifecycleNode::SharedPtr node = std::make_shared("test_node"); tf2_ros::Buffer tf(node->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + nav2_costmap_2d::LayeredCostmap layers("map", false, false); layer.initialize(&layers, "test_layer", &tf, node, nullptr);