started tf_cb

Author: lamprecht

CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   geometry_msgs
   sensor_msgs
+  tf2_msgs
 )
 
 #find_package(OpenCV REQUIRED)
@@ -116,7 +117,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS roscpp tf std_msgs geometry_msgs sensor_msgs
+  CATKIN_DEPENDS roscpp tf std_msgs geometry_msgs sensor_msgs tf2_msgs
 #  DEPENDS system_lib
 )
 # |^ rviz_visual_tools deleted in catkin depends.

data/tf_example1.bag

@@ -0,0 +1,19 @@
+#include <iostream>
+
+// macro to convert a string to a double
+#define SSTR(x) static_cast<std::ostringstream&>(std::ostringstream() << std::dec << x).str()
+
+//macro to round a value to 2 digits
+#define ROUND2(x) std::round(x * 100) / 100
+#define ROUND3(x) std::round(x * 1000) / 1000
+
+
+
+namespace tf_mapping
+{
+
+int getNumOfString(std::string str);
+
+
+
+}

include/tf_mapping/helper.h

@@ -8,7 +8,9 @@
 // #include <tf/transform_listener.h>
 
 // messages
-// #include <geometry_msgs/PoseStamped.h>
+//#include <geometry_msgs/TransformStamped.h>
+#include <tf2_msgs/TFMessage.h>
+
 // #include <geometry_msgs/PoseArray.h>
 // #include <sensor_msgs/CameraInfo.h>
 // #include <sensor_msgs/image_encodings.h>
@@ -28,6 +30,7 @@ class TfMappingNode
 
 protected:
   // ROS API callbacks
+  void input_tfCb(const tf2_msgs::TFMessageConstPtr& msg);
   // void imageCb(const sensor_msgs::ImageConstPtr& image);
   // void updateParamsCb(const std_msgs::Float64& msg);
 
@@ -36,19 +39,23 @@ class TfMappingNode
 
 
   // class members
+  int first_tf_id_;
+  int tf_counter_;
+  bool first_tf_detected_;
+  tf2_msgs::TFMessageConstPtr tf_msg_;
   //tf::TransformListener tf_listener_;
   //Detector::Ptr detector_;
 
   //sensor_msgs::CameraInfo camera_info_;
 
   // parameters (set in yaml file)
-  std::string world_frame_;
+  //std::string world_frame_;
 
 
   // subscriber
   // image_transport::Subscriber img_sub_;
   // ros::Subscriber             init_pos_sub_;  // remove position error!
-  // ros::Subscriber              update_params_sub_; 
+  ros::Subscriber              input_tf_sub_; 
 
   // publisher
   // image_transport::Publisher binary_img_pub_;