Fawkes API
Fawkes Development Version
|
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 }