transform_merger.cpp
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     //update value
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   // set ignore tf
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 }


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Fri Sep 8 2017 03:38:56