Fawkes API  Fawkes Development Version
static_transforms_thread.cpp
00001 
00002 /***************************************************************************
00003  *  static_transform_thread.cpp - Static transform publisher thread
00004  *
00005  *  Created: Tue Oct 25 16:36:04 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "static_transforms_thread.h"
00023 
00024 #include <utils/time/time.h>
00025 #include <set>
00026 #include <memory>
00027 
00028 using namespace fawkes;
00029 
00030 /** @class StaticTransformsThread "static_transforms_thread.h"
00031  * Thread to regularly publish static transforms.
00032  * This thread runs at the sensor hook and publishes a set of
00033  * transforms. The transforms are set in the configuration and
00034  * are static at run-time. Only the timestamp is updated between
00035  * writes.
00036  * @author Tim Niemueller
00037  */
00038 
00039 /** Constructor. */
00040 StaticTransformsThread::StaticTransformsThread()
00041   : Thread("StaticTransformsThread", Thread::OPMODE_WAITFORWAKEUP),
00042     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
00043     TransformAspect(TransformAspect::ONLY_PUBLISHER, "static")
00044 {
00045   __last_update = new Time();
00046 }
00047 
00048 
00049 /** Destructor. */
00050 StaticTransformsThread::~StaticTransformsThread()
00051 {
00052   delete __last_update;
00053 }
00054 
00055 
00056 void
00057 StaticTransformsThread::init()
00058 {
00059   std::set<std::string> transforms;
00060   std::set<std::string> ignored_transforms;
00061 
00062   __cfg_update_interval = 1.0;
00063   try  {
00064     __cfg_update_interval = config->get_float("/plugins/static-transforms/update-interval");
00065   } catch (Exception &e) {} // ignored, use default
00066 
00067   std::string prefix = "/plugins/static-transforms/transforms/";
00068   std::auto_ptr<Configuration::ValueIterator> i(config->search(prefix.c_str()));
00069   while (i->next()) {
00070     std::string cfg_name = std::string(i->path()).substr(prefix.length());
00071     cfg_name = cfg_name.substr(0, cfg_name.find("/"));
00072 
00073     if ( (transforms.find(cfg_name) == transforms.end()) &&
00074          (ignored_transforms.find(cfg_name) == ignored_transforms.end()) ) {
00075 
00076       std::string cfg_prefix = prefix + cfg_name + "/";
00077       
00078       bool active = true;
00079       try {
00080         active = config->get_bool((cfg_prefix + "active").c_str());
00081       } catch (Exception &e) {} // ignored, assume enabled
00082 
00083       if (active) {
00084         try {
00085           std::string frame = config->get_string((cfg_prefix + "frame").c_str());
00086           std::string child_frame =
00087             config->get_string((cfg_prefix + "child_frame").c_str());
00088 
00089           float tx = 0., ty = 0., tz = 0.;
00090           if (config->exists((cfg_prefix + "trans_x").c_str()) ||
00091               config->exists((cfg_prefix + "trans_y").c_str()) ||
00092               config->exists((cfg_prefix + "trans_z").c_str()))
00093           {
00094             tx = config->get_float((cfg_prefix + "trans_x").c_str());
00095             ty = config->get_float((cfg_prefix + "trans_y").c_str());
00096             tz = config->get_float((cfg_prefix + "trans_z").c_str());
00097           } // else assume no translation
00098 
00099           bool use_quaternion = false;
00100           float rx = 0., ry = 0., rz = 0., rw = 1.,
00101             ryaw = 0., rpitch = 0., rroll = 0.;
00102 
00103           if (config->exists((cfg_prefix + "rot_x").c_str()) ||
00104               config->exists((cfg_prefix + "rot_y").c_str()) ||
00105               config->exists((cfg_prefix + "rot_z").c_str()) ||
00106               config->exists((cfg_prefix + "rot_w").c_str()) )
00107           {
00108             use_quaternion = true;
00109             rx = config->get_float((cfg_prefix + "rot_x").c_str());
00110             ry = config->get_float((cfg_prefix + "rot_y").c_str());
00111             rz = config->get_float((cfg_prefix + "rot_z").c_str());
00112             rw = config->get_float((cfg_prefix + "rot_w").c_str());
00113 
00114           } else if (config->exists((cfg_prefix + "rot_roll").c_str()) ||
00115                      config->exists((cfg_prefix + "rot_pitch").c_str()) ||
00116                      config->exists((cfg_prefix + "rot_yaw").c_str()))
00117           {
00118             ryaw   = config->get_float((cfg_prefix + "rot_yaw").c_str());
00119             rpitch = config->get_float((cfg_prefix + "rot_pitch").c_str());
00120             rroll  = config->get_float((cfg_prefix + "rot_roll").c_str());
00121           } // else assume no rotation
00122 
00123           if (frame == child_frame) {
00124             throw Exception("Parent and child frames may not be the same");
00125           }
00126 
00127           try {
00128             Entry e;
00129             e.name = cfg_name;
00130 
00131             fawkes::Time time(clock);
00132             if (use_quaternion) {
00133               tf::Quaternion q(rx, ry, rz, rw);
00134               tf::assert_quaternion_valid(q);
00135               tf::Transform t(q, tf::Vector3(tx, ty, tz));
00136               e.transform = new tf::StampedTransform(t, time, frame, child_frame);
00137             } else {
00138               tf::Quaternion q; q.setEulerZYX(ryaw, rpitch, rroll);
00139               tf::Transform t(q, tf::Vector3(tx, ty, tz));
00140               e.transform = new tf::StampedTransform(t, time, frame, child_frame);
00141             }
00142 
00143             tf::Quaternion q = e.transform->getRotation();
00144 
00145             tf::assert_quaternion_valid(q);
00146 
00147             tf::Vector3 &v = e.transform->getOrigin();
00148             logger->log_debug(name(), "Adding transform '%s' (%s -> %s): "
00149                               "T(%f,%f,%f)  Q(%f,%f,%f,%f)", e.name.c_str(),
00150                               e.transform->frame_id.c_str(),
00151                               e.transform->child_frame_id.c_str(),
00152                               v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
00153 
00154             __entries.push_back(e);
00155           } catch (Exception &e) {
00156             std::list<Entry>::iterator i;
00157             for (i = __entries.begin(); i != __entries.end(); ++i) {
00158               delete i->transform;
00159             }
00160             __entries.clear();
00161             throw;
00162           }
00163           
00164         } catch (Exception &e) {
00165           e.prepend("Transform %s: wrong or incomplete transform data",
00166                     cfg_name.c_str());
00167           throw;
00168         }
00169         
00170         transforms.insert(cfg_name);
00171       } else {
00172         //printf("Ignoring laser config %s\n", cfg_name.c_str());
00173         ignored_transforms.insert(cfg_name);
00174       }
00175     }
00176   }
00177   
00178   if ( __entries.empty() ) {
00179     throw Exception("No transforms configured");
00180   }
00181 
00182   __last_update->set_clock(clock);
00183   __last_update->set_time(0, 0);
00184 }
00185 
00186 
00187 void
00188 StaticTransformsThread::finalize()
00189 {
00190   std::list<Entry>::iterator i;
00191   for (i = __entries.begin(); i != __entries.end(); ++i) {
00192     delete i->transform;
00193   }
00194   __entries.clear();
00195 }
00196 
00197 
00198 void
00199 StaticTransformsThread::loop()
00200 {
00201   fawkes::Time now(clock);
00202   if ((now - __last_update) > __cfg_update_interval) {
00203     __last_update->stamp();
00204 
00205     // date time stamps slightly into the future so they are valid
00206     // for longer and need less frequent updates.
00207     fawkes::Time timestamp = now + (__cfg_update_interval * 1.1);
00208 
00209     std::list<Entry>::iterator i;
00210     for (i = __entries.begin(); i != __entries.end(); ++i) {
00211       i->transform->stamp = timestamp;
00212       tf_publisher->send_transform(*(i->transform));
00213     }
00214   }
00215 }