diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt
index 097d0916..58a2b9f6 100644
--- a/gmapping/CMakeLists.txt
+++ b/gmapping/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8)
 project(gmapping)
 
-find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
+find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf2 tf2_geometry_msgs tf2_ros rosbag_storage)
 
 find_package(Boost REQUIRED signals)
 
diff --git a/gmapping/package.xml b/gmapping/package.xml
index 59939dfb..1219391d 100644
--- a/gmapping/package.xml
+++ b/gmapping/package.xml
@@ -15,17 +15,21 @@
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
   <build_depend>nav_msgs</build_depend>
+  <build_depend>nodelet</build_depend>
   <build_depend>openslam_gmapping</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rostest</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>nodelet</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
 
   <run_depend>nav_msgs</run_depend>
+  <run_depend>nodelet</run_depend>
   <run_depend>openslam_gmapping</run_depend>
   <run_depend>roscpp</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>nodelet</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <run_depend>tf2_ros</run_depend>
   
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp
index bd9977c3..247cce49 100644
--- a/gmapping/src/slam_gmapping.cpp
+++ b/gmapping/src/slam_gmapping.cpp
@@ -114,6 +114,9 @@ Initial map dimensions and resolution:
 #include "ros/ros.h"
 #include "ros/console.h"
 #include "nav_msgs/MapMetaData.h"
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_msgs/TFMessage.h>
 
 #include "gmapping/sensor/sensor_range/rangesensor.h"
 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
@@ -126,27 +129,41 @@ Initial map dimensions and resolution:
 // compute linear index for given map coords
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
+/* This function is only useful to have the whole code work
+ * with old rosbags that have trailing slashes for their frames
+ */
+inline
+std::string stripSlash(const std::string& in)
+{
+  std::string out = in;
+  if ( ( !in.empty() ) && (in[0] == '/') )
+    out.erase(0,1);
+  return out;
+}
+
 SlamGMapping::SlamGMapping():
-  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
   laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
 {
+  tfL_.reset(new tf2_ros::TransformListener(tf_));
+  map_to_odom_.setIdentity();
   seed_ = time(NULL);
   init();
 }
 
 SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
-  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
   laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
 {
+  tfL_.reset(new tf2_ros::TransformListener(tf_));
+  map_to_odom_.setIdentity();
   seed_ = time(NULL);
   init();
 }
 
 SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
-  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
   laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
-  seed_(seed), tf_(ros::Duration(max_duration_buffer))
+  seed_(seed), tf_(ros::Duration(max_duration_buffer)), tfL_(new tf2_ros::TransformListener(tf_))
 {
+  map_to_odom_.setIdentity();
   init();
 }
 
@@ -160,7 +177,7 @@ void SlamGMapping::init()
   gsp_ = new GMapping::GridSlamProcessor();
   ROS_ASSERT(gsp_);
 
-  tfB_ = new tf::TransformBroadcaster();
+  tfB_ = new tf2_ros::TransformBroadcaster();
   ROS_ASSERT(tfB_);
 
   gsp_laser_ = NULL;
@@ -176,10 +193,13 @@ void SlamGMapping::init()
     throttle_scans_ = 1;
   if(!private_nh_.getParam("base_frame", base_frame_))
     base_frame_ = "base_link";
+  base_frame_ = stripSlash(base_frame_);
   if(!private_nh_.getParam("map_frame", map_frame_))
     map_frame_ = "map";
+  map_frame_ = stripSlash(map_frame_);
   if(!private_nh_.getParam("odom_frame", odom_frame_))
     odom_frame_ = "odom";
+  odom_frame_ = stripSlash(odom_frame_);
 
   private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
 
@@ -260,7 +280,7 @@ void SlamGMapping::startLiveSlam()
   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
-  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
+  scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5, node_);
   scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
 
   transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
@@ -287,15 +307,11 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
   std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
   foreach(rosbag::MessageInstance const m, viewall)
   {
-    tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
+    tf2_msgs::TFMessage::ConstPtr cur_tf = m.instantiate<tf2_msgs::TFMessage>();
     if (cur_tf != NULL) {
       for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
       {
-        geometry_msgs::TransformStamped transformStamped;
-        tf::StampedTransform stampedTf;
-        transformStamped = cur_tf->transforms[i];
-        tf::transformStampedMsgToTF(transformStamped, stampedTf);
-        tf_.setTransform(stampedTf);
+        tf_.setTransform(cur_tf->transforms[i], "slam_gmapping");
       }
     }
 
@@ -318,8 +334,8 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
     {
       try
       {
-        tf::StampedTransform t;
-        tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
+        tf_.lookupTransform(stripSlash(s_queue.front().first->header.frame_id),
+                            odom_frame_, s_queue.front().first->header.stamp);
         this->laserCallback(s_queue.front().first);
         s_queue.pop();
       }
@@ -369,22 +385,22 @@ bool
 SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
 {
   // Get the pose of the centered laser at the right time
-  centered_laser_pose_.stamp_ = t;
+  centered_laser_pose_.header.stamp = t;
   // Get the laser's pose that is centered
-  tf::Stamped<tf::Transform> odom_pose;
+  geometry_msgs::PoseStamped odom_pose;
   try
   {
-    tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
+    tf_.transform(centered_laser_pose_, odom_pose, odom_frame_);
   }
-  catch(tf::TransformException e)
+  catch(tf2::TransformException e)
   {
     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
     return false;
   }
-  double yaw = tf::getYaw(odom_pose.getRotation());
+  double yaw = tf2::getYaw(odom_pose.pose.orientation);
 
-  gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
-                                      odom_pose.getOrigin().y(),
+  gmap_pose = GMapping::OrientedPoint(odom_pose.pose.position.x,
+                                      odom_pose.pose.position.y,
                                       yaw);
   return true;
 }
@@ -392,18 +408,20 @@ SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t
 bool
 SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
 {
-  laser_frame_ = scan.header.frame_id;
+  laser_frame_ = stripSlash(scan.header.frame_id);
   // Get the laser's pose, relative to base.
-  tf::Stamped<tf::Pose> ident;
-  tf::Stamped<tf::Transform> laser_pose;
-  ident.setIdentity();
-  ident.frame_id_ = laser_frame_;
-  ident.stamp_ = scan.header.stamp;
+  geometry_msgs::PoseStamped laser_pose;
   try
   {
-    tf_.transformPose(base_frame_, ident, laser_pose);
+    geometry_msgs::PoseStamped ident;
+    ident.header.stamp = scan.header.stamp;
+    ident.header.frame_id = laser_frame_;
+    tf2::Transform transform;
+    transform.setIdentity();
+    tf2::toMsg(transform, ident.pose);
+    tf_.transform(ident, laser_pose, base_frame_);
   }
-  catch(tf::TransformException e)
+  catch(tf2::TransformException e)
   {
     ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
              e.what());
@@ -411,16 +429,17 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
   }
 
   // create a point 1m above the laser position and transform it into the laser-frame
-  tf::Vector3 v;
-  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
-  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
-                                      base_frame_);
+  geometry_msgs::PointStamped up;
+  up.header.stamp = scan.header.stamp;
+  up.header.frame_id = base_frame_;
+  up.point.x = up.point.y = 0;
+  up.point.z = 1 + laser_pose.pose.position.z;
   try
   {
-    tf_.transformPoint(laser_frame_, up, up);
-    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
+    tf_.transform(up, up, laser_frame_);
+    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.point.z);
   }
-  catch(tf::TransformException& e)
+  catch(tf2::TransformException& e)
   {
     ROS_WARN("Unable to determine orientation of laser: %s",
              e.what());
@@ -428,10 +447,10 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
   }
   
   // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
-  if (fabs(fabs(up.z()) - 1) > 0.001)
+  if (fabs(fabs(up.point.z) - 1) > 0.001)
   {
     ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
-                 up.z());
+                 up.point.z);
     return false;
   }
 
@@ -439,20 +458,25 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
 
   double angle_center = (scan.angle_min + scan.angle_max)/2;
 
-  if (up.z() > 0)
+  centered_laser_pose_.header.frame_id = laser_frame_;
+  centered_laser_pose_.header.stamp = ros::Time::now();
+  tf2::Quaternion q;
+  if (up.point.z > 0)
   {
     do_reverse_range_ = scan.angle_min > scan.angle_max;
-    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
-                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+    q.setEuler(angle_center, 0, 0);
     ROS_INFO("Laser is mounted upwards.");
   }
   else
   {
     do_reverse_range_ = scan.angle_min < scan.angle_max;
-    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
-                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+    q.setEuler(-angle_center, 0, M_PI);
     ROS_INFO("Laser is mounted upside down.");
   }
+  centered_laser_pose_.pose.position.x = 0;
+  centered_laser_pose_.pose.position.y = 0;
+  centered_laser_pose_.pose.position.z = 0;
+  convert(q, centered_laser_pose_.pose.orientation);
 
   // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
   laser_angles_.resize(scan.ranges.size());
@@ -621,8 +645,11 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
 
-    tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
-    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
+    tf2::Quaternion q;
+    q.setRPY(0, 0, mpose.theta);
+    tf2::Transform laser_to_map = tf2::Transform(q, tf2::Vector3(mpose.x, mpose.y, 0.0)).inverse();
+    q.setRPY(0, 0, odom_pose.theta);
+    tf2::Transform odom_to_laser = tf2::Transform(q, tf2::Vector3(odom_pose.x, odom_pose.y, 0.0));
 
     map_to_odom_mutex_.lock();
     map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
@@ -762,7 +789,7 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
 
   //make sure to set the header information on the map
   map_.map.header.stamp = ros::Time::now();
-  map_.map.header.frame_id = tf_.resolve( map_frame_ );
+  map_.map.header.frame_id = map_frame_;
 
   sst_.publish(map_.map);
   sstm_.publish(map_.map.info);
@@ -786,6 +813,12 @@ void SlamGMapping::publishTransform()
 {
   map_to_odom_mutex_.lock();
   ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
-  tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
+  geometry_msgs::TransformStamped transform;
+  transform.header.frame_id = map_frame_;
+  transform.header.stamp = tf_expiration;
+  transform.child_frame_id = odom_frame_;
+  transform.transform = tf2::toMsg(map_to_odom_);
+
+  tfB_->sendTransform( transform );
   map_to_odom_mutex_.unlock();
 }
diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
index ae622b95..cfc0c5c4 100644
--- a/gmapping/src/slam_gmapping.h
+++ b/gmapping/src/slam_gmapping.h
@@ -20,10 +20,12 @@
 #include "sensor_msgs/LaserScan.h"
 #include "std_msgs/Float64.h"
 #include "nav_msgs/GetMap.h"
-#include "tf/transform_listener.h"
-#include "tf/transform_broadcaster.h"
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include "message_filters/subscriber.h"
-#include "tf/message_filter.h"
+#include <tf2_ros/message_filter.h>
 
 #include "gmapping/gridfastslam/gridslamprocessor.h"
 #include "gmapping/sensor/sensor_base/sensor.h"
@@ -54,10 +56,11 @@ class SlamGMapping
     ros::Publisher sst_;
     ros::Publisher sstm_;
     ros::ServiceServer ss_;
-    tf::TransformListener tf_;
+    tf2_ros::Buffer tf_;
+    boost::shared_ptr<tf2_ros::TransformListener> tfL_;
     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
-    tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
-    tf::TransformBroadcaster* tfB_;
+    tf2_ros::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
+    tf2_ros::TransformBroadcaster* tfB_;
 
     GMapping::GridSlamProcessor* gsp_;
     GMapping::RangeSensor* gsp_laser_;
@@ -65,7 +68,7 @@ class SlamGMapping
     // symmetrical bounds as that's what gmapping expects)
     std::vector<double> laser_angles_;
     // The pose, in the original laser frame, of the corresponding centered laser with z facing up
-    tf::Stamped<tf::Pose> centered_laser_pose_;
+    geometry_msgs::PoseStamped centered_laser_pose_;
     // Depending on the order of the elements in the scan and the orientation of the scan frame,
     // We might need to change the order of the scan
     bool do_reverse_range_;
@@ -78,7 +81,7 @@ class SlamGMapping
     nav_msgs::GetMap::Response map_;
 
     ros::Duration map_update_interval_;
-    tf::Transform map_to_odom_;
+    tf2::Transform map_to_odom_;
     boost::mutex map_to_odom_mutex_;
     boost::mutex map_mutex_;
 
diff --git a/gmapping/src/tftest.cpp b/gmapping/src/tftest.cpp
index 965c10ef..747e4e90 100644
--- a/gmapping/src/tftest.cpp
+++ b/gmapping/src/tftest.cpp
@@ -1,7 +1,7 @@
 #include <cstdio>
 #include "sensor_msgs/LaserScan.h"
 #include "ros/node.h"
-#include "tf/transform_listener.h"
+#include <tf2_ros/transform_listener.h>
 
 class Test
 {
@@ -9,8 +9,8 @@ class Test
     Test()
     {
       node_ = new ros::Node("test");
-      tf_ = new tf::TransformListener(*node_, true, 
-                                      ros::Duration(10));
+      tf_.reset(new tf2::Buffer(ros::Duration(10)));
+      tfl_ = new tf2_ros::TransformListener(*tf_, *node_, true);
       tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2));
 
       node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
@@ -52,7 +52,8 @@ class Test
 
   private:
     ros::Node* node_;
-    tf::TransformListener* tf_;
+    tf::TransformListener* tfl_;
+    boost::shared_ptr<tf2_ros::Buffer> tf_;
     sensor_msgs::LaserScan scan_;
 };
 
diff --git a/gmapping/test/basic_localization_stage_replay.launch b/gmapping/test/basic_localization_stage_replay.launch
index 83963805..19fa5a1b 100644
--- a/gmapping/test/basic_localization_stage_replay.launch
+++ b/gmapping/test/basic_localization_stage_replay.launch
@@ -4,5 +4,5 @@
   <!--test time-limit="30" test-name="basic_localization_stage" pkg="gmapping" 
         type="test_map.py" args="90.0"/-->
   <!-- TODO: Fix the rtest to work with replay, for now it's not working -->
-  <test time-limit="250" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
+  <test time-limit="300" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
 </launch>
diff --git a/gmapping/test/basic_localization_upside_down.launch b/gmapping/test/basic_localization_upside_down.launch
index 052468f3..998678f2 100644
--- a/gmapping/test/basic_localization_upside_down.launch
+++ b/gmapping/test/basic_localization_upside_down.launch
@@ -1,5 +1,5 @@
 <launch>
   <node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
         args="--bag_filename $(find gmapping)/test/test_upside_down.bag --scan_topic /laserscan/fix/raw _base_frame:=base_floor"/>
-  <test time-limit="190" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
+  <test time-limit="250" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
 </launch>