Skip to content

Commit 000863d

Browse files
2linkthefire2linkthefire
committed
fixing static layer update bounds with different fram
Signed-off-by: 2linkthefire <byepanda@163.com>
1 parent 7c5d0de commit 000863d

File tree

6 files changed

+78
-40
lines changed

6 files changed

+78
-40
lines changed

‎nav2_costmap_2d/plugins/static_layer.cpp

Lines changed: 45 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -363,15 +363,53 @@ StaticLayer::updateBounds(
363363

364364
useExtraBounds(min_x, min_y, max_x, max_y);
365365

366-
double wx, wy;
366+
// Might even be in a different frame
367+
if (map_frame_ == global_frame_) {
368+
double wx, wy;
369+
370+
mapToWorld(x_, y_, wx, wy);
371+
*min_x = std::min(wx, *min_x);
372+
*min_y = std::min(wy, *min_y);
373+
374+
mapToWorld(x_ + width_, y_ + height_, wx, wy);
375+
*max_x = std::max(wx, *max_x);
376+
*max_y = std::max(wy, *max_y);
377+
} else {
378+
geometry_msgs::msg::TransformStamped transform;
379+
try {
380+
transform = tf_->lookupTransform(
381+
global_frame_, map_frame_, tf2::TimePointZero,
382+
transform_tolerance_);
383+
} catch (tf2::TransformException & ex) {
384+
RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
385+
return;
386+
}
387+
tf2::Transform tf2_transform;
388+
tf2::fromMsg(transform.transform, tf2_transform);
389+
390+
double wx, wy;
391+
mapToWorld(x_, y_, wx, wy);
392+
tf2::Vector3 down_left(wx, wy, 0);
393+
394+
mapToWorld(x_ + width_, y_, wx, wy);
395+
tf2::Vector3 down_right(wx, wy, 0);
396+
397+
mapToWorld(x_, y_ + height_, wx, wy);
398+
tf2::Vector3 up_left(wx, wy, 0);
367399

368-
mapToWorld(x_, y_, wx, wy);
369-
*min_x = std::min(wx, *min_x);
370-
*min_y = std::min(wy, *min_y);
400+
mapToWorld(x_ + width_, y_ + height_, wx, wy);
401+
tf2::Vector3 up_right(wx, wy, 0);
371402

372-
mapToWorld(x_ + width_, y_ + height_, wx, wy);
373-
*max_x = std::max(wx, *max_x);
374-
*max_y = std::max(wy, *max_y);
403+
down_left = tf2_transform * down_left;
404+
down_right = tf2_transform * down_right;
405+
up_left = tf2_transform * up_left;
406+
up_right = tf2_transform * up_right;
407+
408+
*min_x = std::min({*min_x, down_left.x(), down_right.x(), up_left.x(), up_right.x()});
409+
*min_y = std::min({*min_y, down_left.y(), down_right.y(), up_left.y(), up_right.y()});
410+
*max_x = std::max({*max_x, down_left.x(), down_right.x(), up_left.x(), up_right.x()});
411+
*max_y = std::max({*max_y, down_left.y(), down_right.y(), up_left.y(), up_right.y()});
412+
}
375413

376414
has_updated_data_ = false;
377415

‎nav2_costmap_2d/test/integration/inflation_tests.cpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -201,7 +201,7 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove)
201201
{
202202
initNode(4.1);
203203
tf2_ros::Buffer tf(node_->get_clock());
204-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
204+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
205205
layers.resizeMap(10, 10, 1, 0, 0);
206206

207207
// Footprint with inscribed radius = 2.1
@@ -233,7 +233,7 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns)
233233
{
234234
initNode(4.1);
235235
tf2_ros::Buffer tf(node_->get_clock());
236-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
236+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
237237
layers.resizeMap(10, 10, 1, 0, 0);
238238

239239
// Footprint with inscribed radius = 2.1
@@ -269,7 +269,7 @@ TEST_F(TestNode, testInflationInUnknown)
269269
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
270270

271271
tf2_ros::Buffer tf(node_->get_clock());
272-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
272+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
273273
layers.resizeMap(9, 9, 1, 0, 0);
274274

275275
// Footprint with inscribed radius = 2.1
@@ -305,7 +305,7 @@ TEST_F(TestNode, testInflationAroundUnknown)
305305
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
306306

307307
tf2_ros::Buffer tf(node_->get_clock());
308-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
308+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
309309
layers.resizeMap(10, 10, 1, 0, 0);
310310

311311
// Footprint with inscribed radius = 2.1
@@ -330,7 +330,7 @@ TEST_F(TestNode, testCostFunctionCorrectness)
330330
{
331331
initNode(10.5);
332332
tf2_ros::Buffer tf(node_->get_clock());
333-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
333+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
334334

335335
layers.resizeMap(100, 100, 1, 0, 0);
336336
// Footprint with inscribed radius = 5.0
@@ -405,7 +405,7 @@ TEST_F(TestNode, testInflationOrderCorrectness)
405405
const double inflation_radius = 4.1;
406406
initNode(inflation_radius);
407407
tf2_ros::Buffer tf(node_->get_clock());
408-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
408+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
409409
layers.resizeMap(10, 10, 1, 0, 0);
410410

411411
// Footprint with inscribed radius = 2.1
@@ -438,7 +438,7 @@ TEST_F(TestNode, testInflation)
438438
{
439439
initNode(1);
440440
tf2_ros::Buffer tf(node_->get_clock());
441-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
441+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
442442

443443
// Footprint with inscribed radius = 2.1
444444
// circumscribed radius = 3.1
@@ -516,7 +516,7 @@ TEST_F(TestNode, testInflation2)
516516
{
517517
initNode(1);
518518
tf2_ros::Buffer tf(node_->get_clock());
519-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
519+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
520520

521521
// Footprint with inscribed radius = 2.1
522522
// circumscribed radius = 3.1
@@ -554,7 +554,7 @@ TEST_F(TestNode, testInflation3)
554554
{
555555
initNode(3);
556556
tf2_ros::Buffer tf(node_->get_clock());
557-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
557+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
558558
layers.resizeMap(10, 10, 1, 0, 0);
559559

560560
// 1 2 3

‎nav2_costmap_2d/test/integration/obstacle_tests.cpp

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,7 @@ class TestNode : public ::testing::Test
148148
TEST_F(TestNode, testRaytracing) {
149149
tf2_ros::Buffer tf(node_->get_clock());
150150

151-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
151+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
152152
addStaticLayer(layers, tf, node_);
153153
auto olayer = addObstacleLayer(layers, tf, node_);
154154

@@ -170,7 +170,7 @@ TEST_F(TestNode, testRaytracing) {
170170
*/
171171
TEST_F(TestNode, testRaytracing2) {
172172
tf2_ros::Buffer tf(node_->get_clock());
173-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
173+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
174174
addStaticLayer(layers, tf, node_);
175175
auto olayer = addObstacleLayer(layers, tf, node_);
176176

@@ -227,7 +227,7 @@ TEST_F(TestNode, testWaveInterference) {
227227
tf2_ros::Buffer tf(node_->get_clock());
228228
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
229229
// Start with an empty map, no rolling window, tracking unknown
230-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
230+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
231231
layers.resizeMap(10, 10, 1, 0, 0);
232232
auto olayer = addObstacleLayer(layers, tf, node_);
233233

@@ -255,7 +255,7 @@ TEST_F(TestNode, testWaveInterference) {
255255
TEST_F(TestNode, testZThreshold) {
256256
tf2_ros::Buffer tf(node_->get_clock());
257257
// Start with an empty map
258-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
258+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
259259
layers.resizeMap(10, 10, 1, 0, 0);
260260

261261
auto olayer = addObstacleLayer(layers, tf, node_);
@@ -275,7 +275,7 @@ TEST_F(TestNode, testZThreshold) {
275275
*/
276276
TEST_F(TestNode, testDynamicObstacles) {
277277
tf2_ros::Buffer tf(node_->get_clock());
278-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
278+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
279279
addStaticLayer(layers, tf, node_);
280280

281281
auto olayer = addObstacleLayer(layers, tf, node_);
@@ -300,7 +300,7 @@ TEST_F(TestNode, testDynamicObstacles) {
300300
*/
301301
TEST_F(TestNode, testMultipleAdditions) {
302302
tf2_ros::Buffer tf(node_->get_clock());
303-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
303+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
304304
addStaticLayer(layers, tf, node_);
305305

306306
auto olayer = addObstacleLayer(layers, tf, node_);
@@ -319,7 +319,7 @@ TEST_F(TestNode, testMultipleAdditions) {
319319
*/
320320
TEST_F(TestNode, testRepeatedResets) {
321321
tf2_ros::Buffer tf(node_->get_clock());
322-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
322+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
323323

324324
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
325325
addStaticLayer(layers, tf, node_, slayer);
@@ -370,7 +370,7 @@ TEST_F(TestNode, testRepeatedResets) {
370370
TEST_F(TestNode, testRaytracing) {
371371
tf2_ros::Buffer tf(node_->get_clock());
372372

373-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
373+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
374374
layers.resizeMap(10, 10, 1, 0, 0);
375375

376376
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
@@ -555,7 +555,7 @@ TEST_F(TestNode, testMaxCombinationMethod) {
555555
tf2_ros::Buffer tf(node_->get_clock());
556556

557557
// Create a costmap with full unknown space
558-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
558+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
559559
layers.resizeMap(10, 10, 1, 0, 0);
560560

561561
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
@@ -600,7 +600,7 @@ TEST_F(TestNodeWithoutUnknownOverwrite, testMaxWithoutUnknownOverwriteCombinatio
600600
tf2_ros::Buffer tf(node_->get_clock());
601601

602602
// Create a costmap with full unknown space
603-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
603+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
604604
layers.resizeMap(10, 10, 1, 0, 0);
605605

606606
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;

‎nav2_costmap_2d/test/integration/plugin_container_tests.cpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,7 @@ class TestNode : public ::testing::Test
158158
TEST_F(TestNode, testObstacleLayers) {
159159
tf2_ros::Buffer tf(node_->get_clock());
160160

161-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
161+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
162162
layers.resizeMap(10, 10, 1, 0, 0);
163163

164164
std::shared_ptr<nav2_costmap_2d::PluginContainerLayer> pclayer_a = nullptr;
@@ -191,7 +191,7 @@ TEST_F(TestNode, testObstacleAndStaticLayers) {
191191
node_->declare_parameter("pclayer_a.static.map_topic",
192192
rclcpp::ParameterValue(std::string("map")));
193193

194-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
194+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
195195

196196
layers.resizeMap(10, 10, 1, 0, 0);
197197

@@ -232,7 +232,7 @@ TEST_F(TestNode, testDifferentInflationLayers) {
232232
node_->declare_parameter("pclayer_b.inflation.inflation_radius",
233233
rclcpp::ParameterValue(1.0));
234234

235-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
235+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
236236

237237
layers.resizeMap(10, 10, 1, 0, 0);
238238

@@ -281,7 +281,7 @@ TEST_F(TestNode, testDifferentInflationLayers2) {
281281
node_->declare_parameter("pclayer_a.inflation.inflation_radius",
282282
rclcpp::ParameterValue(1.0));
283283

284-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
284+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
285285

286286
layers.resizeMap(10, 10, 1, 0, 0);
287287

@@ -336,7 +336,7 @@ TEST_F(TestNode, testResetting) {
336336
node_->declare_parameter("pclayer_b.inflation.inflation_radius",
337337
rclcpp::ParameterValue(1.0));
338338

339-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
339+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
340340

341341
layers.resizeMap(10, 10, 1, 0, 0);
342342

@@ -407,7 +407,7 @@ TEST_F(TestNode, testClearing) {
407407
node_->declare_parameter("pclayer_b.inflation.inflation_radius",
408408
rclcpp::ParameterValue(1.0));
409409

410-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
410+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
411411

412412
layers.resizeMap(10, 10, 1, 0, 0);
413413

@@ -473,7 +473,7 @@ TEST_F(TestNode, testOverwriteCombinationMethods) {
473473
node_->declare_parameter("pclayer_b.inflation.inflation_radius",
474474
rclcpp::ParameterValue(1.0));
475475

476-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
476+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
477477

478478
layers.resizeMap(10, 10, 1, 0, 0);
479479

@@ -534,7 +534,7 @@ TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) {
534534
node_->declare_parameter("pclayer_b.inflation.inflation_radius",
535535
rclcpp::ParameterValue(1.0));
536536

537-
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
537+
nav2_costmap_2d::LayeredCostmap layers("map", false, true);
538538

539539
layers.resizeMap(10, 10, 1, 0, 0);
540540

@@ -580,7 +580,7 @@ TEST_F(TestNode, testWithoutUnknownOverwriteCombinationMethods) {
580580
TEST_F(TestNode, testClearable) {
581581
tf2_ros::Buffer tf(node_->get_clock());
582582

583-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
583+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
584584

585585
std::shared_ptr<nav2_costmap_2d::PluginContainerLayer> pclayer_a = nullptr;
586586
addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a");

‎nav2_costmap_2d/test/integration/range_tests.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ TEST_F(TestNode, testClearingAtMaxRange) {
140140
transform.transform.translation.x = 2;
141141
tf_.setTransform(transform, "default_authority", true);
142142

143-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
143+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
144144
layers.resizeMap(10, 10, 1, 0, 0);
145145

146146
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
@@ -180,7 +180,7 @@ TEST_F(TestNode, testProbabilisticModelForward) {
180180
transform.transform.translation.x = 2;
181181
tf_.setTransform(transform, "default_authority", true);
182182

183-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
183+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
184184
layers.resizeMap(10, 10, 1, 0, 0);
185185

186186
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
@@ -233,7 +233,7 @@ TEST_F(TestNode, testProbabilisticModelDownward) {
233233
transform.transform.translation.x = 2;
234234
tf_.setTransform(transform, "default_authority", true);
235235

236-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
236+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
237237
layers.resizeMap(10, 10, 1, 0, 0);
238238

239239
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};

‎nav2_costmap_2d/test/unit/declare_parameter_test.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ TEST(DeclareParameter, useValidParameter)
3434
nav2_util::LifecycleNode::SharedPtr node =
3535
std::make_shared<nav2_util::LifecycleNode>("test_node");
3636
tf2_ros::Buffer tf(node->get_clock());
37-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
37+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
3838

3939
layer.initialize(&layers, "test_layer", &tf, node, nullptr);
4040

@@ -53,7 +53,7 @@ TEST(DeclareParameter, useInvalidParameter)
5353
nav2_util::LifecycleNode::SharedPtr node =
5454
std::make_shared<nav2_util::LifecycleNode>("test_node");
5555
tf2_ros::Buffer tf(node->get_clock());
56-
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
56+
nav2_costmap_2d::LayeredCostmap layers("map", false, false);
5757

5858
layer.initialize(&layers, "test_layer", &tf, node, nullptr);
5959

0 commit comments

Comments
 (0)