TFManager.h
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           ros_rt_wmp              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V0.1B  15/09/11
00005  *
00006  *
00007  *  File: ./src/TFManager.h
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2011, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 
00038 #ifndef TFMANAGER_H_
00039 #define TFMANAGER_H_
00040 
00041 #include <ros/ros.h>
00042 #include <sstream>
00043 #include <boost/thread.hpp>
00044 #include <vector>
00045 #include <ostream>
00046 #include "config.h"
00047 #include "Manager.h"
00048 #include <tf/tfMessage.h>
00049 #include <boost/shared_ptr.hpp>
00050 
00051 
00052 class TFManager: public Manager {
00053         struct info_t{
00054                 tf::tfMessage tfm;
00055                 bool sent;
00056                 ros::Time ts;
00057         };
00058         std::map<std::string, info_t> tf_map;
00059         ros::Subscriber sub;
00060         pthread_mutex_t mtx;
00061 public:
00062 
00063         TFManager(ros::NodeHandle * n, int port, std::string source, std::string destination, unsigned char priority, bool broadcast) : Manager(n,port,"/tf", priority) {
00064                 amIstatic = true;
00065                 setBroadcast(broadcast);
00066                 setDestination(destination);
00067                 setSource(source);
00068         }
00069 
00070         TFManager(ros::NodeHandle * n, int port, std::string source,unsigned char priority, bool broadcast) : Manager(n,port,"/tf", priority) {
00071                 amIdst = true;
00072                 amIstatic = false;
00073                 setBroadcast(broadcast);
00074                 setSource(source);
00075                 init_param();
00076         }
00077         virtual void fillDestination() {
00078                 std::string val;
00079                 n->getParamCached(param_dest, val);
00080                 ROSWMP_DEBUG(stderr,"Get param cached: %s\n",val.c_str());
00081                 setDestination(val);
00082         }
00083 
00084         virtual void startRX() {
00085                 if (amIsrc){
00086                         sub = n->subscribe(name, 10, &TFManager::callback, this);
00087                         boost::thread(boost::bind(&TFManager::run, this));
00088                         ROS_INFO("TF Tx subscribed (%s)",name.c_str());
00089                 }
00090                 if (amIdst){
00091                         boost::thread(boost::bind(&TFManager::waitNetData, this));
00092                 }
00093                 pthread_mutex_init(&mtx,0);
00094         }
00095 
00096         void callback(const boost::shared_ptr<tf::tfMessage const> & message) {
00097                 pthread_mutex_lock(&mtx);
00098                 std::vector< geometry_msgs::TransformStamped > ros_vec;
00099 //              message->get_transforms_vec (ros_vec);
00100                 std::string tf_prefix;
00101                 bool found = n->getParamCached("/tf_prefix",tf_prefix);
00102 
00103                 if (!found){
00104                         ROS_WARN("Please specify a tf_prefix present: %s",tf_prefix.c_str());
00105                 }
00106                 if (ros_vec.at(0).header.frame_id.find(tf_prefix)!=std::string::npos or ros_vec.at(0).child_frame_id.find(tf_prefix) != std::string::npos){
00107                         std::string s = ros_vec.at(0).header.frame_id + "-" + ros_vec.at(0).child_frame_id;
00108                         tf_map[s].tfm = *message;
00109                         tf_map[s].sent = false;
00110                         tf_map[s].ts = ros::Time::now();
00111                 }else{
00112                         ROSWMP_DEBUG(stderr,"Discarding foreign prefix %s", tf_prefix.c_str());
00113                 }
00114                 pthread_mutex_unlock(&mtx);
00115         }
00116 
00117         void waitNetData(){
00118                 char * rxbuff;
00119                 signed char priority;
00120                 unsigned int size;
00121                 unsigned char src;
00122                 tf::tfMessage m;
00123                 ros::Publisher publisher = n->advertise<tf::tfMessage> ("/tf",1);
00124 
00125                 while (1){
00126                         int offset = 0;
00127                         ROSWMP_DEBUG(stderr,"Popping msg\n");
00128                         int id = wmpPopData(port,&rxbuff,&size,&src,&priority);
00129                         ROSWMP_DEBUG(stderr,"Popped msg of size: %d\n",size);
00130                         flow_t * fw = (flow_t *) rxbuff;
00131                         int npaks = fw->npaks;
00132                         for (int i = 0; i < npaks ; i++){
00133                                 fw = (flow_t *) (rxbuff + offset);
00134                                 ROSWMP_DEBUG(stderr,"Publish %dth TF data fw->len = %d offset = %d size: %d\n", i, fw->len, offset, size);
00135                                 //m.deserialize((uint8_t*) (buff + offset + sizeof(flow_t)));
00136                                 if (!deserialize<tf::tfMessage>(rxbuff + offset + sizeof(flow_t),fw->len,m)){
00137                                         break;
00138                                 }
00139                                 for (unsigned i = 0; i< m.transforms.size(); i++){
00140                                         m.transforms.at(i).header.stamp = ros::Time::now();
00141                                 }
00142                                 publisher.publish(m);
00143                                 offset+= sizeof(flow_t) + fw->len;
00144                         }
00145                         wmpPopDataDone(id);
00146                 }
00147         }
00148         void run(){
00149                 while (1){
00150                         pthread_mutex_lock(&mtx);
00151                         if (tf_map.size() > 0) {
00152 
00153                                 if (!amIstatic){
00154                                         fillDestination();
00155                                 }
00156                                 if (dests.size() > 0) {
00157 
00158                                         int offset = 0, size = 0, npaks = 0;
00159                                         std::map<std::string, info_t>::iterator it;
00160                                         for (it = tf_map.begin(); it != tf_map.end(); it++) {
00161                                                 if (!it->second.sent){
00162                                                         flow_t * fw = (flow_t *) (sbuff + offset);
00163                                                         npaks++;
00164 
00165                                                         int len = serialize<tf::tfMessage> (
00166                                                                 (char*) (sbuff + sizeof(flow_t) + offset),
00167                                                                 it->second.tfm);
00168                                                         it->second.sent = true;
00169 
00170                                                         fw->len = len;
00171                                                         offset += (len + sizeof(flow_t));
00172                                                         size += (len + sizeof(flow_t));
00173                                                 }else{
00174                                                         ROSWMP_DEBUG(stderr,"Discarding already sent\n");
00175                                                 }
00176                                         }
00177                                         flow_t * fw = (flow_t *) (sbuff);
00178                                         fw->npaks = npaks;
00179 
00180                                         if (size > 0) {
00181                                                 int dest = computeBroadcastDest();
00182                                                 wmpPushData(port, sbuff, size, dest, 99);
00183                                         }
00184                                 }
00185                         }
00186                         pthread_mutex_unlock(&mtx);
00187                         usleep(20000);
00188                 }
00189         }
00190 };
00191 #endif


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:11