diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
index 06dd335540d837be863e18bda379489710227111..7ab18481f93976101c6d87eea1bdd65a426560e2 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
@@ -95,6 +95,47 @@ protected:
     };
     std::shared_ptr<QueuedAction_Module> QueuedAction_ModulePtr;
 
+    /**
+     * Thread for broadcasting tf transform
+     */
+    class TfBroadcastThread : public Thread {
+    protected:
+        // Pose information
+        Pose robot;
+        Pose odometry;
+        std::string mapFrame;
+        std::string odomFrame;
+
+        // Operating info
+        double frequency;
+        bool initial;
+        bool operating;
+
+        // Broadcaster
+        tf::TransformBroadcaster *tfBroadcast;
+
+        // Threading
+        Mutex updateMutex;
+
+    public:
+        TfBroadcastThread(tf::TransformBroadcaster *tfBroadcast,
+                std::string mapFrame, std::string odomFrame, double frequency) :
+            Thread("FastSLAMModule::TfBroadcastThread"),
+            tfBroadcast(tfBroadcast),
+            initial(false),
+            operating(false),
+            frequency(frequency),
+            mapFrame(mapFrame),
+            odomFrame(odomFrame) {
+        }
+
+        void setTransform(const Pose& robot, const Pose& odometry);
+
+        void run();
+        void stop();
+    };
+    std::shared_ptr<TfBroadcastThread> broadcastThread;
+
 	virtual void resetMap_action();
 	virtual void loadMap_action(QueuedActionPtr action);
 	virtual void estimatePose_action(QueuedActionPtr action);
diff --git a/crosbot_fastslam/src/module.cpp b/crosbot_fastslam/src/module.cpp
index 1f9e98ce98b77a3a1a420f30ab24c78140804857..1dac8bab8b22319dac6d069b908646a9f79c4d14 100644
--- a/crosbot_fastslam/src/module.cpp
+++ b/crosbot_fastslam/src/module.cpp
@@ -109,6 +109,11 @@ void FastSLAMModule::initialize(ros::NodeHandle& nh) {
     // Crosbot commands
     crosbotCommand = new crosbot::CrosbotCommand("", false, new _CommandCallbackFastSLAM(this));
 
+    // Broadcasting thread
+    double tfFrequency = 0.1;
+    paramNH.param<double>("tf_frequency", tfFrequency, 0.1);
+    broadcastThread = std::make_shared<TfBroadcastThread>(&tfBroadcast, mapFrame, odomFrame, tfFrequency);
+
     // read configuration/parameters
     ConfigElementPtr config = new ROSConfigElement(paramNH);
     configure(config);
@@ -119,6 +124,11 @@ void FastSLAMModule::initialize(ros::NodeHandle& nh) {
 void FastSLAMModule::start() {
     // Start fastslam
     FastSLAMMap::start();
+
+    // Start broadcast thread
+    //if (!broadcastThread->isAlive()) {
+    //    broadcastThread->start();
+    //}
 }
 
 void FastSLAMModule::stop() {
@@ -128,6 +138,9 @@ void FastSLAMModule::stop() {
     gridPub.shutdown();
     historyPub.shutdown();
 
+    // Stop broadcast
+    //broadcastThread->stop();
+
     // Stop fastslam
     FastSLAMMap::stop();
 }
@@ -204,6 +217,50 @@ bool FastSLAMModule::service_setMap(nav_msgs::SetMapRequest &req, nav_msgs::SetM
     return true;
 }
 
+void FastSLAMModule::TfBroadcastThread::setTransform(const Pose& robot, const Pose& odometry) {
+    Lock lock(updateMutex);
+    this->robot = robot;
+    this->odometry = odometry;
+}
+
+void FastSLAMModule::TfBroadcastThread::run() {
+    operating = true;
+
+    ros::Rate rate(frequency);
+    while (operating) {
+        // Broadcast latest update
+        {
+            Lock lock(updateMutex);
+
+            // The latest information is always the "current" transform
+            // This is necessary since if fastslam isn't updating (or takes time)
+            // Then the transforms get too old
+            ros::Time time = ros::Time::now();
+
+            geometry_msgs::TransformStamped ts;
+            ts.header.stamp = time;
+            ts.header.frame_id = mapFrame;
+            ts.child_frame_id = odomFrame;
+
+            Pose pose = robot.toTF() * odometry.toTF().inverse();
+
+            ts.transform.translation.x = pose.position.x;
+            ts.transform.translation.y = pose.position.y;
+            ts.transform.translation.z = pose.position.z;
+            ts.transform.rotation = pose.orientation.toROS();
+
+            //ROS_INFO("%s broadcast tf", LOG_START);
+            tfBroadcast->sendTransform(ts);
+        }
+
+        rate.sleep();
+    }
+}
+
+void FastSLAMModule::TfBroadcastThread::stop() {
+    operating = false;
+}
+
 void FastSLAMModule::publishTransform(const Pose& robot, const Pose& odometry, const Time& time) {
     geometry_msgs::TransformStamped ts;
     ts.header.stamp = time.toROS();
@@ -218,6 +275,8 @@ void FastSLAMModule::publishTransform(const Pose& robot, const Pose& odometry, c
     ts.transform.rotation = pose.orientation.toROS();
     //ROS_INFO("%s broadcast tf", LOG_START);
     tfBroadcast.sendTransform(ts);
+
+    //broadcastThread->setTransform(robot, odometry);
 }
 
 void FastSLAMModule::motionTracked(MapPtr map) {