$search
00001 00002 00003 // yliu 06/26/2011 00004 00005 #include "ros/ros.h" 00006 #include <geometry_msgs/TransformStamped.h> 00007 #include <tf/transform_broadcaster.h> 00008 #include <fstream> 00009 #include <string> 00010 #include <time.h> 00011 #include <sstream> 00012 #include <iomanip> 00013 #include <kdl/frames.hpp> 00014 00015 #include <rosbag/bag.h> 00016 00017 00018 00019 00020 00021 static std::list<geometry_msgs::TransformStamped> T_list; 00022 00023 00024 void MyCallback(const geometry_msgs::TransformStamped::ConstPtr& msg) 00025 { 00026 //printf("From: %s\n", msg->header.frame_id.c_str()); 00027 //printf("To : %s\n", msg->child_frame_id.c_str()); 00028 //printf("Q = [%f, %f , %f, %f]\n",msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w); 00029 //printf("p = [%f, %f, %f]\n\n", msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z); 00030 00031 //printf("call back\n"); 00032 00033 00034 if (T_list.size() > 0) 00035 { 00036 for (std::list<geometry_msgs::TransformStamped>::iterator it = T_list.begin(); it != T_list.end(); ++it) 00037 { 00038 if ( msg->header.frame_id == it->header.frame_id && msg->child_frame_id == it->child_frame_id ) 00039 { 00040 // replace 00041 //printf("replacing\n"); 00042 it->transform = msg->transform; 00043 } 00044 else 00045 { 00046 //printf("pushing back\n"); 00047 T_list.push_back( *msg ); 00048 } 00049 } 00050 } 00051 else 00052 { 00053 //printf("pushing back\n"); 00054 T_list.push_back( *msg ); 00055 } 00056 00057 } 00058 00059 00060 00061 int main(int argc, char **argv) 00062 { 00063 ros::init(argc, argv, "camera_pose_static_transform_tf_broadcaster"); 00064 ros::NodeHandle n; 00065 00066 ros::Subscriber sub = n.subscribe("camera_pose_static_transform_update", 10, MyCallback); 00067 00068 00069 tf::TransformBroadcaster br; 00070 00071 ros::Duration sleeper(100/1000.0); // 10 hz 00072 00073 00074 // Is this a race condition ?? -- NO 00075 while(n.ok() ) //to see whether it's yet time to exit. 00076 { 00077 //printf("looping\n"); 00078 ros::spinOnce(); 00079 for (std::list<geometry_msgs::TransformStamped>::iterator it = T_list.begin(); it != T_list.end(); ++it) 00080 { 00081 tf::Transform transform; 00082 std::string parent_frame_id; 00083 std::string child_frame_id; 00084 00085 transform.setOrigin( tf::Vector3(it->transform.translation.x, it->transform.translation.y, it->transform.translation.z) ); 00086 transform.setRotation( tf::Quaternion(it->transform.rotation.x, it->transform.rotation.y, it->transform.rotation.z, it->transform.rotation.w) ); 00087 parent_frame_id = it->header.frame_id; 00088 child_frame_id = it->child_frame_id; 00089 00090 if (parent_frame_id.compare(child_frame_id) != 0) //target frame and source frame are not the same 00091 { 00092 //printf("%s\n", parent_frame_id.c_str()); 00093 //printf("%s\n", child_frame_id.c_str()); 00094 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), parent_frame_id, child_frame_id)); 00095 //printf("..\n"); 00096 } 00097 //printf("%s - %s \n", parent_frame_id.c_str(), child_frame_id.c_str()); 00098 00099 } 00100 sleeper.sleep(); 00101 } 00102 00103 // write to rosbag 00104 rosbag::Bag bag; 00105 bag.open("transform_list.bag", rosbag::bagmode::Write); 00106 00107 for (std::list<geometry_msgs::TransformStamped>::iterator it = T_list.begin(); it != T_list.end(); ++it) 00108 { 00109 bag.write("camera_pose_static_transform_update", ros::Time::now(), *it); 00110 } 00111 bag.close(); 00112 00113 00114 return 0; 00115 00116 }