transform_playback.cpp
Go to the documentation of this file.
00001 
00002 
00003 // yliu 07/30/2011
00004 
00005 // playback calibrated transformations
00006 
00007 #include "ros/ros.h"
00008 #include <geometry_msgs/TransformStamped.h>
00009 #include <tf/transform_broadcaster.h>
00010 #include <fstream>
00011 #include <string>
00012 #include <time.h>
00013 #include <sstream>
00014 #include <iomanip>
00015 #include <kdl/frames.hpp>
00016 
00017 #include <rosbag/bag.h>
00018 #include <rosbag/view.h>
00019 #include <boost/foreach.hpp>
00020 
00021 
00022 
00023 
00024 
00025 static std::list<geometry_msgs::TransformStamped> T_list;
00026 
00027 
00028 int main(int argc, char **argv)
00029 {
00030 
00031         ros::init(argc, argv, "transform_playback_node");
00032         printf("%d\n", argc);
00033         std::string bag_name; 
00034         printf("transform_playback_node: play ");
00035         if ( argc < 2)
00036         {
00037                 bag_name = "/transform_list.bag";  
00038         }
00039         else
00040         {
00041                 bag_name = argv[1];
00042                 printf("%s\n", argv[1]);
00043         }
00044                
00045         ros::NodeHandle n;
00046         
00047         // read the bag
00048         rosbag::Bag bag;
00049         bag.open(bag_name, rosbag::bagmode::Read);
00050 
00051         std::vector<std::string> topics;
00052         topics.push_back(std::string("camera_pose_static_transform_update"));
00053 
00054         rosbag::View view(bag, rosbag::TopicQuery(topics));
00055 
00056         BOOST_FOREACH(rosbag::MessageInstance const m, view)
00057         {
00058                 geometry_msgs::TransformStamped::ConstPtr ts = m.instantiate<geometry_msgs::TransformStamped>();
00059                 if (ts != NULL)
00060                         T_list.push_back( *ts);
00061 
00062         }
00063 
00064         bag.close();
00065 
00066 
00067         tf::TransformBroadcaster br;
00068 
00069         ros::Duration sleeper(100/1000.0); // 10 hz
00070 
00071 
00072         //
00073         while(n.ok() )  //to see whether it's yet time to exit.
00074         {
00075                 ros::spinOnce();
00076                 for (std::list<geometry_msgs::TransformStamped>::iterator it = T_list.begin(); it != T_list.end(); ++it)
00077                 {
00078                         tf::Transform transform;
00079                         std::string parent_frame_id;
00080                         std::string child_frame_id;
00081 
00082                         transform.setOrigin( tf::Vector3(it->transform.translation.x, it->transform.translation.y, it->transform.translation.z) );
00083                         transform.setRotation( tf::Quaternion(it->transform.rotation.x, it->transform.rotation.y, it->transform.rotation.z, it->transform.rotation.w) );
00084                         parent_frame_id = it->header.frame_id;
00085                         child_frame_id = it->child_frame_id;
00086 
00087                         if (parent_frame_id.compare(child_frame_id) != 0)  //target frame and source frame are not the same
00088                         {       
00089                                 //printf("%s\n", parent_frame_id.c_str());
00090                                 //printf("%s\n", child_frame_id.c_str());                       
00091                                 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), parent_frame_id, child_frame_id));   
00092                                 //printf("..\n");
00093                         }
00094                         //printf("%s - %s \n", parent_frame_id.c_str(), child_frame_id.c_str());
00095                         
00096                 }
00097                 sleeper.sleep();
00098         }
00099 
00100         return 0;
00101 
00102 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_toolkits
Author(s): Yiping Liu
autogenerated on Thu Aug 15 2013 12:02:52