added different frame_id in publish pose

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

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);