00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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
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