Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <tf/tf.h>
00003 #include <tf/transform_listener.h>
00004 #include <tf/transform_broadcaster.h>
00005 #include <iostream>
00006 #include <map>
00007 #include <vector>
00008
00009 std::map<std::string, geometry_msgs::TransformStamped> tf_map;
00010
00011 void transformCallback(const tf::tfMessage::ConstPtr& msg){
00012 std::pair<std::map<std::string, geometry_msgs::TransformStamped>::iterator, bool> ret;
00013 for(int i=0; i<msg->transforms.size(); i++){
00014 geometry_msgs::TransformStamped tfs = msg->transforms[i];
00015 ret = tf_map.insert( std::map<std::string, geometry_msgs::TransformStamped>::value_type(tfs.child_frame_id, tfs) );
00016
00017
00018 if(!ret.second){
00019 tf_map[tfs.child_frame_id] = tfs;
00020 }
00021 }
00022 }
00023
00024 int main(int argc, char** argv)
00025 {
00026 ros::init(argc, argv, "transform_merger");
00027 ros::NodeHandle n;
00028 ros::NodeHandle pnh_("~");
00029 tf::TransformListener tfl_;
00030
00031 ros::Publisher pub_ = pnh_.advertise<tf::tfMessage> ("/tf_merged", 1);
00032 ros::Subscriber sub_ = pnh_.subscribe<tf::tfMessage>
00033 ("/tf", 100, transformCallback);
00034
00035 double loop_hz;
00036 pnh_.param("loop_hz", loop_hz, 1.0 );
00037 ROS_INFO_STREAM("loop_hz:" << loop_hz);
00038
00039 ros::Rate rate(loop_hz);
00040
00041
00042 std::vector< std::string > ignore_tf_vec;
00043
00044
00045 XmlRpc::XmlRpcValue v;
00046 pnh_.param("transform_config", v, v);
00047 if(v.hasMember("ignore_tf")){
00048 XmlRpc::XmlRpcValue ignore_v = v["ignore_tf"];
00049 ROS_INFO_STREAM("ignore following transform");
00050 for(int i=0; i< ignore_v.size(); i++){
00051 ignore_tf_vec.push_back(ignore_v[i]);
00052 ROS_INFO_STREAM("ignore: " << ignore_v[i]);
00053 }
00054 }
00055
00056
00057 while (ros::ok())
00058 {
00059 tf::tfMessage tf_msg;
00060 std::map<std::string, geometry_msgs::TransformStamped>::iterator it = tf_map.begin();
00061 while( it != tf_map.end() )
00062 {
00063
00064 std::vector< std::string >::iterator ignore_it = find( ignore_tf_vec.begin(), ignore_tf_vec.end(), it->second.child_frame_id );
00065 if(ignore_it == ignore_tf_vec.end()){
00066 tf_msg.transforms.push_back( it->second );
00067 }
00068 ++it;
00069 }
00070 pub_.publish(tf_msg);
00071 tf_map.clear();
00072 rate.sleep();
00073 ros::spinOnce();
00074 }
00075 }