update to tf3

plugins/directional_layer.cpp

@@ -1,7 +1,7 @@
 #include <costmap_2d/directional_layer.h>
 #include <costmap_2d/data_convert.h>
-#include <tf2/convert.h>
-#include <tf2/utils.h>
+#include <tf3/convert.h>
+#include <tf3/utils.h>
 
 #include <boost/dll/alias.hpp>
 
@@ -134,8 +134,8 @@ namespace costmap_2d
         return false;
       }
       // Convert to yaw
-      tf2::Quaternion quaternion = convertQuaternion(p.pose.orientation);
-      double theta = tf2::getYaw(quaternion);
+      tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation);
+      double theta = tf3::getYaw(quaternion);
       unsigned char value = laneFilter(mx, my, theta);
       if (get_success)
       {
@@ -399,27 +399,27 @@ namespace costmap_2d
 
   bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
   {
-    tf2::TransformStampedMsg transformMsg;
+    tf3::TransformStampedMsg transformMsg;
     try
     {
-      transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf2::Time());
+      transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf3::Time());
         
-      // listener_.lookupTransform(map_frame_, base_frame_id_, tf2::Time(), transform);
+      // listener_.lookupTransform(map_frame_, base_frame_id_, tf3::Time(), transform);
     }
-    catch (tf2::TransformException &ex)
+    catch (tf3::TransformException &ex)
     {
       printf("%s\n", ex.what());
       return false;
     }
     // Copy map data given proper transformations
-    tf2::Transform tf2_transform;
+    tf3::Transform tf2_transform;
     tf2_transform = convertToTf2Transform(transformMsg.transform);
     x = tf2_transform.getOrigin().x();
     y = tf2_transform.getOrigin().y();
     // Extract the rotation as a quaternion from the transform
-    tf2::Quaternion rotation = tf2_transform.getRotation();
+    tf3::Quaternion rotation = tf2_transform.getRotation();
     // Convert the quaternion to a yaw angle (in radians)
-    yaw = tf2::getYaw(rotation);
+    yaw = tf3::getYaw(rotation);
     return true;
   }
 

plugins/obstacle_layer.cpp

@@ -2,8 +2,8 @@
 #include <costmap_2d/costmap_math.h>
 #include <costmap_2d/utils.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
-#include <tf2/convert.h>
-#include <tf2/utils.h>
+#include <tf3/convert.h>
+#include <tf3/utils.h>
 #include <boost/dll/alias.hpp>
 
 using costmap_2d::NO_INFORMATION;
@@ -154,7 +154,7 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
   {
     projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
   }
-  catch (tf2::TransformException &ex)
+  catch (tf3::TransformException &ex)
   {
     printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
              ex.what());
@@ -197,7 +197,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
   {
     projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
   }
-  catch (tf2::TransformException &ex)
+  catch (tf3::TransformException &ex)
   {
     printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
              global_frame_.c_str(), ex.what());

plugins/static_layer.cpp

@@ -3,8 +3,8 @@
 #include <costmap_2d/costmap_math.h>
 #include <costmap_2d/data_convert.h>
 #include <costmap_2d/utils.h>
-#include <tf2/convert.h>
-#include <tf2/utils.h>
+#include <tf3/convert.h>
+#include <tf3/utils.h>
 #include <boost/dll/alias.hpp>
 
 
@@ -283,25 +283,25 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
     double wx, wy;
     // Might even be in a different frame
     // geometry_msgs::TransformStamped transform;
-    tf2::TransformStampedMsg transformMsg;
+    tf3::TransformStampedMsg transformMsg;
     try
     {
       // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
-      bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time());
-      if(!status) throw tf2::TransformException("[static_layer] Cannot transform");
+      bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
+      if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
       transformMsg = tf_->lookupTransform(map_frame_, 
                                           global_frame_,
-                                          tf2::Time());
+                                          tf3::Time());
     }
-    catch (tf2::TransformException ex)
+    catch (tf3::TransformException ex)
     {
       printf("%s", ex.what());
       return;
     }
     // Copy map data given proper transformations
-    tf2::Transform tf2_transform;
+    tf3::Transform tf2_transform;
     tf2_transform = convertToTf2Transform(transformMsg.transform);
-    // tf2::convert(transformMsg.transform, tf2_transform);
+    // tf3::convert(transformMsg.transform, tf2_transform);
     for (unsigned int i = min_i; i < max_i; ++i)
     {
       for (unsigned int j = min_j; j < max_j; ++j)
@@ -309,7 +309,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
         // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
         layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
         // Transform from global_frame_ to map_frame_
-        tf2::Vector3 p(wx, wy, 0);
+        tf3::Vector3 p(wx, wy, 0);
         p = tf2_transform*p;
         // Set master_grid with cell from map
         if (worldToMap(p.x(), p.y(), mx, my))