diff --git a/CMakeLists.txt b/CMakeLists.txt
index bd507875618ae70cb4092b1098a565006bf78289..340ee74300393fbd32c6f922c1c6e921c7842ce7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   std_msgs
   tf
+  tf_conversions
   dynamic_reconfigure
 
 )
@@ -167,7 +168,21 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 add_dependencies(visualizer ${PROJECT_NAME}_gencfg)
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(visualizer ${wolf_LIBRARIES})
+target_link_libraries(subscriber_${PROJECT_NAME}
+                      ${catkin_LIBRARIES}
+                      ${CERES_LIBRARIES}
+                      ${wolf_LIBRARIES}
+                      yaml-cpp
+                      dl
+                      )
+
+target_link_libraries(publisher_${PROJECT_NAME}
+                      ${catkin_LIBRARIES}
+                      ${CERES_LIBRARIES}
+                      ${wolf_LIBRARIES}
+                      yaml-cpp
+                      dl
+                      )
 
 target_link_libraries(${PROJECT_NAME}
                       visualizer
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index ae27c1ef00cf99b831c80a41182d828019baa307..11fc3937928ac16df25970ceb80e7f187a4aac4b 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -113,7 +113,7 @@ void PublisherPose::publishDerived()
     }
 
     // Change frame
-    if (frame_id_ != map_frame_id_)
+    if (frame_id_ != map_frame_id_ and listenTf())
     {
         p = t_frame_ + q_frame_ * p;
         q = q_frame_ * q;
@@ -154,9 +154,9 @@ void PublisherPose::publishPose(const geometry_msgs::Pose pose, const ros::Time&
 bool PublisherPose::listenTf()
 {
     tf::StampedTransform T;
-    if ( tfl_.waitForTransform(map_frame_id_, frame_id_, ros::Time(0), ros::Duration(0.01)) )
+    if ( tfl_.waitForTransform(frame_id_, map_frame_id_, ros::Time(0), ros::Duration(0.01)) )
     {
-        tfl_.lookupTransform(map_frame_id_, frame_id_, ros::Time(0), T);
+        tfl_.lookupTransform(frame_id_, map_frame_id_, ros::Time(0), T);
 
         Eigen::Matrix3d R;
         tf::matrixTFToEigen(T.getBasis(), R);