diff --git a/rotational_recovery/CMakeLists.txt b/rotational_recovery/CMakeLists.txt
index de23287cacf92f17b2a531031c791608a5d6e246..756624a608a61fc95e975633ee8cd24122987d30 100644
--- a/rotational_recovery/CMakeLists.txt
+++ b/rotational_recovery/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(rotational_recovery)
 
 # Find ROS dependencies
-set(THIS_PACKAGE_ROS_DEPS nav_core costmap_2d geometry_msgs pluginlib base_local_planner)
+set(THIS_PACKAGE_ROS_DEPS nav_core costmap_2d geometry_msgs pluginlib base_local_planner tf2_ros)
 find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS})
 
 include_directories(include ${catkin_INCLUDE_DIRS})
diff --git a/rotational_recovery/include/rotational_recovery/rotational_recovery.h b/rotational_recovery/include/rotational_recovery/rotational_recovery.h
index 15f16640d9a51d53eeb13839a55f7aef1d0abc54..234688e62c2198733e3a70061634dc24204e39e5 100644
--- a/rotational_recovery/include/rotational_recovery/rotational_recovery.h
+++ b/rotational_recovery/include/rotational_recovery/rotational_recovery.h
@@ -5,6 +5,8 @@
 #include <costmap_2d/costmap_2d_ros.h>
 #include <base_local_planner/costmap_model.h>
 #include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Twist.h>
+#include <tf2_ros/transform_listener.h>
 
 namespace rotational_recovery
 {
@@ -22,7 +24,7 @@ namespace rotational_recovery
     ~RotationalRecovery();
 
     /// Initialize the parameters of the behavior
-    void initialize (std::string n, tf::TransformListener* tf,
+    void initialize (std::string n, tf2_ros::Buffer* buffer,
       costmap_2d::Costmap2DROS* global_costmap,
       costmap_2d::Costmap2DROS* local_costmap);
 
@@ -46,7 +48,7 @@ namespace rotational_recovery
       costmap_2d::Costmap2DROS* global_costmap_;
       costmap_2d::Costmap2DROS* local_costmap_;
       std::string name_;
-      tf::TransformListener* tf_;
+      tf2_ros::Buffer* buffer_;
       ros::Publisher pub_;
       bool initialized_;
 
diff --git a/rotational_recovery/package.xml b/rotational_recovery/package.xml
index 02e78e7cfa78b7d2cd71067ac07887b9cd57be00..2d8778a71c80a5a9edf6c7a6fbb3c1aa8cb69f95 100644
--- a/rotational_recovery/package.xml
+++ b/rotational_recovery/package.xml
@@ -16,6 +16,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav_core</depend>
   <depend>pluginlib</depend>
+  <depend>tf2_ros</depend>
 
   <export>
     <nav_core plugin="${prefix}/rotational_recovery.xml" />
diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index edc43487f29d9ca04ce9cb2399bb5a35ef290084..57d824d9a76b906eea5fd934ef4568eb9484706e 100644
--- a/rotational_recovery/src/rotational_recovery.cpp
+++ b/rotational_recovery/src/rotational_recovery.cpp
@@ -13,7 +13,7 @@ namespace rotational_recovery
 
   // Constructor
   RotationalRecovery::RotationalRecovery () :
-  global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
+  global_costmap_(NULL), local_costmap_(NULL), buffer_(NULL), initialized_(false)
   {
     base_frame_twist_.linear.x = 0.0;
     base_frame_twist_.linear.y = 0.0;
@@ -31,12 +31,12 @@ namespace rotational_recovery
   }
 
   // Initialize the recovery reading all necessary params
-  void RotationalRecovery::initialize (std::string name, tf::TransformListener* tf,
+  void RotationalRecovery::initialize (std::string name, tf2_ros::Buffer* buffer,
     costmap_2d::Costmap2DROS* global_cmap, costmap_2d::Costmap2DROS* local_cmap)
     {
       ROS_ASSERT(!initialized_);
       name_ = name;
-      tf_ = tf;
+      buffer_ = buffer;
       local_costmap_ = local_cmap;
       global_costmap_ = global_cmap;
       world_model_ = new base_local_planner::CostmapModel(*global_costmap_->getCostmap());
@@ -131,12 +131,16 @@ namespace rotational_recovery
   // Get pose in local costmap frame
   geometry_msgs::Pose2D RotationalRecovery::getCurrentLocalPose () const
   {
-    tf::Stamped<tf::Pose> p;
+    geometry_msgs::PoseStamped p;
     local_costmap_->getRobotPose(p);
     geometry_msgs::Pose2D pose;
-    pose.x = p.getOrigin().x();
-    pose.y = p.getOrigin().y();
-    pose.theta = tf::getYaw(p.getRotation());
+    pose.x = p.pose.position.x;
+    pose.y = p.pose.position.y;
+    tf::Quaternion q(p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w);
+    tf::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+    pose.theta = yaw;
     return pose;
   }
 
diff --git a/stepback_and_steerturn_recovery/CMakeLists.txt b/stepback_and_steerturn_recovery/CMakeLists.txt
index 89718892b75c864508dbf941bd97a1249c75a794..8a81797eb00b3ed03c80bf9d16719a7eed126e70 100644
--- a/stepback_and_steerturn_recovery/CMakeLists.txt
+++ b/stepback_and_steerturn_recovery/CMakeLists.txt
@@ -3,20 +3,20 @@ project(stepback_and_steerturn_recovery)
 
 find_package(catkin REQUIRED
         COMPONENTS
-            cmake_modules
             roscpp
-            tf
+            tf2_ros
             costmap_2d
             nav_core
             pluginlib
             base_local_planner
+	    geometry_msgs
         )
 
-find_package(Eigen REQUIRED)
+find_package(Eigen3 REQUIRED)
 include_directories(
     include
     ${catkin_INCLUDE_DIRS}
-    ${EIGEN_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
 )
 add_definitions(${EIGEN_DEFINITIONS})
 
diff --git a/stepback_and_steerturn_recovery/include/stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h b/stepback_and_steerturn_recovery/include/stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h
index 7c2263b16157bd530e6175ce0cbffe9df91b1da3..d0e4731d492cad4a3b9f7b3082438c340d720be5 100644
--- a/stepback_and_steerturn_recovery/include/stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h
+++ b/stepback_and_steerturn_recovery/include/stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h
@@ -35,7 +35,9 @@
 #include <base_local_planner/costmap_model.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Twist.h>
 #include <std_msgs/Bool.h>
+#include <tf2_ros/transform_listener.h>
 
 using std::vector;
 using std::max;
@@ -55,7 +57,7 @@ public:
   ~StepBackAndSteerTurnRecovery();
 
   /// Initialize the parameters of the behavior
-  void initialize (std::string n, tf::TransformListener* tf,
+  void initialize (std::string n, tf2_ros::Buffer* buffer,
                    costmap_2d::Costmap2DROS* global_costmap,
                    costmap_2d::Costmap2DROS* local_costmap);
 
@@ -104,7 +106,7 @@ private:
   costmap_2d::Costmap2DROS* local_costmap_;
   costmap_2d::Costmap2D costmap_; // Copy of local_costmap_, used by world model
   std::string name_;
-  tf::TransformListener* tf_;
+  tf2_ros::Buffer* buffer_;
   ros::Publisher cmd_vel_pub_;
   ros::Publisher recover_run_pub_;
   bool initialized_;
diff --git a/stepback_and_steerturn_recovery/package.xml b/stepback_and_steerturn_recovery/package.xml
index 768345e7ca31e74a7634541e9b619e2de7eb08e6..350e0fef4a5353c362f680bb35fa6d423639096e 100644
--- a/stepback_and_steerturn_recovery/package.xml
+++ b/stepback_and_steerturn_recovery/package.xml
@@ -17,11 +17,12 @@
 
     <buildtool_depend>catkin</buildtool_depend>
     <depend>roscpp</depend>
-    <depend>tf</depend>
+    <depend>tf2_ros</depend>
     <depend>costmap_2d</depend>
     <depend>nav_core</depend>
     <depend>pluginlib</depend>
     <depend>eigen</depend>
+    <depend>geometry_msgs</depend>
     <build_depend>cmake_modules</build_depend>
     <build_depend>base_local_planner</build_depend>
 
diff --git a/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp b/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
index 4682123a4c6e275761954f3ca39881aa0be0d2cd..0c44cf2d99c528220ed4725be2a8816d6ef6f71e 100644
--- a/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
+++ b/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
@@ -33,14 +33,13 @@
 #include <tf/transform_datatypes.h>
 
 // register as a RecoveryBehavior plugin
-PLUGINLIB_DECLARE_CLASS(stepback_and_steerturn_recovery, StepBackAndSteerTurnRecovery, stepback_and_steerturn_recovery::StepBackAndSteerTurnRecovery,
-                        nav_core::RecoveryBehavior)
+PLUGINLIB_EXPORT_CLASS(stepback_and_steerturn_recovery::StepBackAndSteerTurnRecovery,nav_core::RecoveryBehavior)
 
 namespace stepback_and_steerturn_recovery
 {
 
 StepBackAndSteerTurnRecovery::StepBackAndSteerTurnRecovery () :
-  global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
+  global_costmap_(NULL), local_costmap_(NULL), buffer_(NULL), initialized_(false)
 {
     TWIST_STOP.linear.x = 0.0;
     TWIST_STOP.linear.y = 0.0;
@@ -55,12 +54,12 @@ StepBackAndSteerTurnRecovery::~StepBackAndSteerTurnRecovery ()
   delete world_model_;
 }
 
-void StepBackAndSteerTurnRecovery::initialize (std::string name, tf::TransformListener* tf,
+void StepBackAndSteerTurnRecovery::initialize (std::string name, tf2_ros::Buffer* buffer,
                                 costmap_2d::Costmap2DROS* global_cmap, costmap_2d::Costmap2DROS* local_cmap)
 {
   ROS_ASSERT(!initialized_);
   name_ = name;
-  tf_ = tf;
+  buffer_ = buffer;
   local_costmap_ = local_cmap;
   global_costmap_ = global_cmap;
   /*
@@ -237,12 +236,16 @@ geometry_msgs::Twist StepBackAndSteerTurnRecovery::scaleGivenAccelerationLimits
 // Get pose in local costmap frame
 geometry_msgs::Pose2D StepBackAndSteerTurnRecovery::getCurrentLocalPose () const
 {
-  tf::Stamped<tf::Pose> p;
+  geometry_msgs::PoseStamped p;
   local_costmap_->getRobotPose(p);
   geometry_msgs::Pose2D pose;
-  pose.x = p.getOrigin().x();
-  pose.y = p.getOrigin().y();
-  pose.theta = tf::getYaw(p.getRotation());
+  pose.x = p.pose.position.x;
+  pose.y = p.pose.position.y;
+  tf::Quaternion q(p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w);
+  tf::Matrix3x3 m(q);
+  double roll, pitch, yaw;
+  m.getRPY(roll, pitch, yaw);
+  pose.theta = yaw;
   return pose;
 }
 
diff --git a/twist_recovery/CMakeLists.txt b/twist_recovery/CMakeLists.txt
index 92b6278b7ace0f46a5d69ab273b9a7aa635d0aa5..faa3b9bffc632346ac02e16e88647363e0043902 100644
--- a/twist_recovery/CMakeLists.txt
+++ b/twist_recovery/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(twist_recovery)
 
 # Find ROS dependencies
-set(THIS_PACKAGE_ROS_DEPS nav_core costmap_2d geometry_msgs pluginlib base_local_planner)
+set(THIS_PACKAGE_ROS_DEPS nav_core costmap_2d geometry_msgs pluginlib base_local_planner tf2_ros)
 find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS})
 
 include_directories(include ${catkin_INCLUDE_DIRS})
diff --git a/twist_recovery/include/twist_recovery/twist_recovery.h b/twist_recovery/include/twist_recovery/twist_recovery.h
index 4f56f72669e5a7ed810bc400006ab873883d9ab5..712dbf2ac534ca296996a56246351768317cedfa 100644
--- a/twist_recovery/include/twist_recovery/twist_recovery.h
+++ b/twist_recovery/include/twist_recovery/twist_recovery.h
@@ -35,6 +35,8 @@
 #include <costmap_2d/costmap_2d_ros.h>
 #include <base_local_planner/costmap_model.h>
 #include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Twist.h>
+#include <tf2_ros/transform_listener.h>
 
 namespace twist_recovery
 {
@@ -52,7 +54,7 @@ namespace twist_recovery
     ~TwistRecovery();
 
     /// Initialize the parameters of the behavior
-    void initialize (std::string n, tf::TransformListener* tf,
+    void initialize (std::string n, tf2_ros::Buffer* buffer,
       costmap_2d::Costmap2DROS* global_costmap,
       costmap_2d::Costmap2DROS* local_costmap);
 
@@ -73,7 +75,7 @@ namespace twist_recovery
       costmap_2d::Costmap2DROS* global_costmap_;
       costmap_2d::Costmap2DROS* local_costmap_;
       std::string name_;
-      tf::TransformListener* tf_;
+      tf2_ros::Buffer* buffer_;
       ros::Publisher pub_;
       bool initialized_;
 
diff --git a/twist_recovery/package.xml b/twist_recovery/package.xml
index ebd359a943ab81ca154867ed7ce92b2721fb95d6..511e2a896a647209234b5dd26f51eab8e5beadab 100644
--- a/twist_recovery/package.xml
+++ b/twist_recovery/package.xml
@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav_core</depend>
   <depend>pluginlib</depend>
+  <depend>tf2_ros</depend>
 
   <export>
     <nav_core plugin="${prefix}/twist_plugin.xml" />
diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp
index 21659d0723cf7c0058b655aa83871bea2bf847ce..48e29fcfa16137408779f602d10d974c53a45e09 100644
--- a/twist_recovery/src/twist_recovery.cpp
+++ b/twist_recovery/src/twist_recovery.cpp
@@ -43,7 +43,7 @@ namespace twist_recovery
 
   // Constructor
   TwistRecovery::TwistRecovery () :
-  global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
+  global_costmap_(NULL), local_costmap_(NULL), buffer_(NULL), initialized_(false)
   {
     base_frame_twist_.linear.x = 0.0;
     base_frame_twist_.linear.y = 0.0;
@@ -61,12 +61,12 @@ namespace twist_recovery
   }
 
   // Initialize the recovery reading all necessary params
-  void TwistRecovery::initialize (std::string name, tf::TransformListener* tf,
+  void TwistRecovery::initialize (std::string name, tf2_ros::Buffer* buffer,
     costmap_2d::Costmap2DROS* global_cmap, costmap_2d::Costmap2DROS* local_cmap)
     {
       ROS_ASSERT(!initialized_);
       name_ = name;
-      tf_ = tf;
+      buffer_ = buffer;
       local_costmap_ = local_cmap;
       global_costmap_ = global_cmap;
       world_model_ = new base_local_planner::CostmapModel(*global_costmap_->getCostmap());
@@ -195,12 +195,16 @@ namespace twist_recovery
   // Get pose in local costmap frame
   geometry_msgs::Pose2D TwistRecovery::getCurrentLocalPose () const
   {
-    tf::Stamped<tf::Pose> p;
+    geometry_msgs::PoseStamped p;
     local_costmap_->getRobotPose(p);
     geometry_msgs::Pose2D pose;
-    pose.x = p.getOrigin().x();
-    pose.y = p.getOrigin().y();
-    pose.theta = tf::getYaw(p.getRotation());
+    pose.x = p.pose.position.x;
+    pose.y = p.pose.position.y;
+    tf::Quaternion q(p.pose.orientation.x,p.pose.orientation.y,p.pose.orientation.z,p.pose.orientation.w);
+    tf::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+    pose.theta = yaw;
     return pose;
   }