rocon_tf_reconstructor.cpp
Go to the documentation of this file.
00001 #include <rocon_tf_reconstructor/rocon_tf_reconstructor.h>
00002 
00003 namespace rocon {
00004 
00005   RoconTFReconstructor::RoconTFReconstructor()
00006   {
00007     this->nh = ros::NodeHandle("");
00008     init();
00009   }
00010   RoconTFReconstructor::RoconTFReconstructor(ros::NodeHandle& n)
00011   {
00012     this->nh = n;
00013     init();
00014   }
00015 
00016 
00017   RoconTFReconstructor::~RoconTFReconstructor()
00018   {
00019   }
00020 
00021   void RoconTFReconstructor::init()
00022   {
00023     getParams();
00024     setSubscriber();
00025   }
00026 
00027   void RoconTFReconstructor::getParams()
00028   {
00029     this->nh.param<std::string>("list_concert_client",this->sub_client_list_topic,"/concert/list_concert_clients");
00030     this->nh.param<std::string>("robotpose_topic",this->sub_robotpose_topic,"robot_pose");
00031   }
00032 
00033 
00034   void RoconTFReconstructor::setSubscriber()
00035   {
00036     this->sub_clientlist = this->nh.subscribe(this->sub_client_list_topic,10,&RoconTFReconstructor::processClientLists,this);
00037   }
00038 
00039   void RoconTFReconstructor::processClientLists(const concert_msgs::ConcertClients::ConstPtr msg)
00040   {
00041     int i;
00042     std::string name;
00043     std::vector<std::string> client_names;
00044     // TODO : Optimize synchronization using sorting algorithm
00045 
00046     // register newly joined clients
00047     for(i = 0; i <msg->clients.size(); i ++) {
00048       name = msg->clients[i].name;
00049       client_names.push_back(name);
00050 
00051       // if it doesn't exist create subscriber
00052       if(this->sub_clients_pose.find(name) == this->sub_clients_pose.end())
00053       {
00054         std::string topic_name = "/" + name + "/" + this->sub_robotpose_topic;  
00055         ROS_INFO_STREAM("Create Subscriber for : " << name << "\tTopic : " << topic_name);
00056         this->sub_clients_pose[name] = new RoconPoseClient(this->nh,name,this->sub_robotpose_topic);
00057       }
00058     }
00059 
00060     // remove newly left clients
00061     std::map<std::string,RoconPoseClient*>::iterator iter;
00062     for(iter = this->sub_clients_pose.begin(); iter != this->sub_clients_pose.end(); ++iter)
00063     {
00064       std::string key = iter->first;
00065 
00066       // it does not exist in msg client name array. so this client has left concert.
00067       if(std::find(client_names.begin(),client_names.end(),key) == client_names.end())
00068       {
00069         ROS_INFO_STREAM("Remove subscriber of : " << name);
00070         this->sub_clients_pose.erase(key);
00071       }
00072     }
00073   }
00074 
00075   void RoconTFReconstructor::publishClientTF(std::string client_name,geometry_msgs::PoseStamped pose_stamped)
00076   {
00077     geometry_msgs::TransformStamped odom_tf;
00078 
00079     odom_tf.header = pose_stamped.header;
00080     odom_tf.header.stamp = ros::Time::now();
00081     odom_tf.child_frame_id = client_name;
00082     odom_tf.transform.translation.x = pose_stamped.pose.position.x;
00083     odom_tf.transform.translation.y = pose_stamped.pose.position.y;
00084     odom_tf.transform.translation.z = pose_stamped.pose.position.z;
00085     odom_tf.transform.rotation = pose_stamped.pose.orientation;
00086 
00087     this->tf_broadcaster.sendTransform(odom_tf);
00088   }
00089 
00090   void RoconTFReconstructor::spin()
00091   {
00092     ROS_INFO("In Spin!");
00093     RoconPoseClient* rpc;
00094     ros::Rate spin_rate(1000);
00095 
00096     while(ros::ok())
00097     {
00098       ros::spinOnce();
00099       std::map<std::string,RoconPoseClient*>::iterator iter;
00100       for(iter = this->sub_clients_pose.begin(); iter != this->sub_clients_pose.end(); ++iter)
00101       {
00102         rpc = iter->second;
00103         if(rpc->isInitialized()) {
00104           std::string client_name = rpc->getClientName();
00105           geometry_msgs::PoseStamped client_pose = rpc->getPoseStamped();
00106 
00107           publishClientTF(client_name,client_pose);
00108         }
00109       }
00110       spin_rate.sleep();
00111     }
00112   }
00113 }


rocon_tf_reconstructor
Author(s):
autogenerated on Wed Aug 26 2015 15:53:51