Fix static layer not transforming to global frame

costmap_2d/plugins/static_layer.cpp

@@ -278,15 +278,30 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
 
   useExtraBounds(min_x, min_y, max_x, max_y);
 
-  double wx, wy;
+  geometry_msgs::TransformStamped transform;
+  try
+  {
+    transform = tf_->lookupTransform(global_frame_, map_frame_, ros::Time(0));
+  }
+  catch (tf2::TransformException ex)
+  {
+    ROS_ERROR("%s", ex.what());
+    return;
+  }
 
-  mapToWorld(x_, y_, wx, wy);
-  *min_x = std::min(wx, *min_x);
-  *min_y = std::min(wy, *min_y);
+  // corners of the static map in map frame
+  const std::vector<std::pair<double, double>> corners = {
+    { x_, y_ }, { x_, y_ + height_ }, { x_ + width_, y_ + height_ }, { x_ + width_, y_ }
+  };
 
-  mapToWorld(x_ + width_, y_ + height_, wx, wy);
-  *max_x = std::max(wx, *max_x);
-  *max_y = std::max(wy, *max_y);
+  // touch the corners in global frame
+  for (const auto& corner : corners)
+  {
+    geometry_msgs::Point p;
+    mapToWorld(corner.first, corner.second, p.x, p.y);
+    tf2::doTransform(p, p, transform);
+    touch(p.x, p.y, min_x, min_y, max_x, max_y);
+  }
 
   has_updated_data_ = false;
 }

costmap_2d/test/obstacle_tests.cpp

@@ -1,10 +1,10 @@
 /*
  * Copyright (c) 2013, Willow Garage, Inc.
  * All rights reserved.
- * 
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
- * 
+ *
  *     * Redistributions of source code must retain the above copyright
  *       notice, this list of conditions and the following disclaimer.
  *     * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
  *     * Neither the name of the Willow Garage, Inc. nor the names of its
  *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
- * 
+ *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -36,6 +36,7 @@
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/observation_buffer.h>
 #include <costmap_2d/testing_helper.h>
+#include <tf2_ros/transform_listener.h>
 #include <set>
 #include <gtest/gtest.h>
 
@@ -72,18 +73,19 @@ using namespace costmap_2d;
  */
 TEST(costmap, testRaytracing){
   tf2_ros::Buffer tf;
+  tf2_ros::TransformListener tfl(tf);
 
   LayeredCostmap layers("frame", false, false);  // Not rolling window, not tracking unknown
   addStaticLayer(layers, tf);  // This adds the static map
   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
-  
+
   // Add a point at 0, 0, 0
   addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
 
   // This actually puts the LETHAL (254) point in the costmap at (0,0)
   layers.updateMap(0,0,0);  // 0, 0, 0 is robot pose
   //printMap(*(layers.getCostmap()));
-  
+
   int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
 
   // We expect just one obstacle to be added (20 in static map)
@@ -95,6 +97,7 @@ TEST(costmap, testRaytracing){
  */
 TEST(costmap, testRaytracing2){
   tf2_ros::Buffer tf;
+  tf2_ros::TransformListener tfl(tf);
   LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf);
   ObstacleLayer* olayer = addObstacleLayer(layers, tf);
@@ -124,7 +127,7 @@ TEST(costmap, testRaytracing2){
 
   // Change from previous test:
   // No obstacles from the static map will be cleared, so the
-  // net change is +1. 
+  // net change is +1.
   ASSERT_EQ(obs_after, obs_before + 1);
 
   // Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
@@ -202,6 +205,7 @@ TEST(costmap, testZThreshold){
  */
 TEST(costmap, testDynamicObstacles){
   tf2_ros::Buffer tf;
+  tf2_ros::TransformListener tfl(tf);
   LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf);
 
@@ -228,6 +232,7 @@ TEST(costmap, testDynamicObstacles){
  */
 TEST(costmap, testMultipleAdditions){
   tf2_ros::Buffer tf;
+  tf2_ros::TransformListener tfl(tf);
   LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf);
 

costmap_2d/test/obstacle_tests.launch

@@ -1,4 +1,5 @@
 <launch>
+  <node name="static_tf" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 map frame"/>
   <node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
   <test time-limit="300" test-name="obstacle_tests" pkg="costmap_2d" type="obstacle_tests" />