Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * TransformInterface.cpp - Fawkes BlackBoard Interface - TransformInterface 00004 * 00005 * Templated created: Thu Oct 12 10:49:19 2006 00006 * Copyright 2011 Tim Niemueller 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <interfaces/TransformInterface.h> 00025 00026 #include <core/exceptions/software.h> 00027 00028 #include <cstring> 00029 #include <cstdlib> 00030 00031 namespace fawkes { 00032 00033 /** @class TransformInterface <interfaces/TransformInterface.h> 00034 * TransformInterface Fawkes BlackBoard Interface. 00035 * 00036 This interface is used to publish transforms. It aims to be as 00037 compatible as possible with ROS' tf library and is used 00038 extensively by the Fawkes tf library. 00039 00040 For this to work properly it is crucial to have correct 00041 timestamp set (cf. Interface::set_timestamp()). Set this as 00042 close as possible to the time of when the data, from which the 00043 transform is computed, has been acquired. 00044 00045 * @ingroup FawkesInterfaces 00046 */ 00047 00048 00049 00050 /** Constructor */ 00051 TransformInterface::TransformInterface() : Interface() 00052 { 00053 data_size = sizeof(TransformInterface_data_t); 00054 data_ptr = malloc(data_size); 00055 data = (TransformInterface_data_t *)data_ptr; 00056 data_ts = (interface_data_ts_t *)data_ptr; 00057 memset(data_ptr, 0, data_size); 00058 add_fieldinfo(IFT_STRING, "frame", 32, data->frame); 00059 add_fieldinfo(IFT_STRING, "child_frame", 32, data->child_frame); 00060 add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation); 00061 add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation); 00062 unsigned char tmp_hash[] = {0x97, 0xc8, 0x15, 0xe9, 0xcb, 0xd2, 0x33, 0x33, 0xf3, 0xfe, 0x49, 0x72, 0x92, 0x99, 0xd9, 0xeb}; 00063 set_hash(tmp_hash); 00064 } 00065 00066 /** Destructor */ 00067 TransformInterface::~TransformInterface() 00068 { 00069 free(data_ptr); 00070 } 00071 /* Methods */ 00072 /** Get frame value. 00073 * 00074 Parent frame ID. The given transform is relative to the origin 00075 of this coordinate frame. 00076 00077 * @return frame value 00078 */ 00079 char * 00080 TransformInterface::frame() const 00081 { 00082 return data->frame; 00083 } 00084 00085 /** Get maximum length of frame value. 00086 * @return length of frame value, can be length of the array or number of 00087 * maximum number of characters for a string 00088 */ 00089 size_t 00090 TransformInterface::maxlenof_frame() const 00091 { 00092 return 32; 00093 } 00094 00095 /** Set frame value. 00096 * 00097 Parent frame ID. The given transform is relative to the origin 00098 of this coordinate frame. 00099 00100 * @param new_frame new frame value 00101 */ 00102 void 00103 TransformInterface::set_frame(const char * new_frame) 00104 { 00105 strncpy(data->frame, new_frame, sizeof(data->frame)); 00106 data_changed = true; 00107 } 00108 00109 /** Get child_frame value. 00110 * 00111 The ID of the child frame. The child frame's origin is at the 00112 given point in the parent frame denoted by the transform. 00113 00114 * @return child_frame value 00115 */ 00116 char * 00117 TransformInterface::child_frame() const 00118 { 00119 return data->child_frame; 00120 } 00121 00122 /** Get maximum length of child_frame value. 00123 * @return length of child_frame value, can be length of the array or number of 00124 * maximum number of characters for a string 00125 */ 00126 size_t 00127 TransformInterface::maxlenof_child_frame() const 00128 { 00129 return 32; 00130 } 00131 00132 /** Set child_frame value. 00133 * 00134 The ID of the child frame. The child frame's origin is at the 00135 given point in the parent frame denoted by the transform. 00136 00137 * @param new_child_frame new child_frame value 00138 */ 00139 void 00140 TransformInterface::set_child_frame(const char * new_child_frame) 00141 { 00142 strncpy(data->child_frame, new_child_frame, sizeof(data->child_frame)); 00143 data_changed = true; 00144 } 00145 00146 /** Get translation value. 00147 * 00148 This array denotes the translation vector of the transform. The 00149 element indexes are ordered x, y, z, i.e. translation[0] is the 00150 X value of the translation vector. 00151 00152 * @return translation value 00153 */ 00154 double * 00155 TransformInterface::translation() const 00156 { 00157 return data->translation; 00158 } 00159 00160 /** Get translation value at given index. 00161 * 00162 This array denotes the translation vector of the transform. The 00163 element indexes are ordered x, y, z, i.e. translation[0] is the 00164 X value of the translation vector. 00165 00166 * @param index index of value 00167 * @return translation value 00168 * @exception Exception thrown if index is out of bounds 00169 */ 00170 double 00171 TransformInterface::translation(unsigned int index) const 00172 { 00173 if (index > 3) { 00174 throw Exception("Index value %u out of bounds (0..3)", index); 00175 } 00176 return data->translation[index]; 00177 } 00178 00179 /** Get maximum length of translation value. 00180 * @return length of translation value, can be length of the array or number of 00181 * maximum number of characters for a string 00182 */ 00183 size_t 00184 TransformInterface::maxlenof_translation() const 00185 { 00186 return 3; 00187 } 00188 00189 /** Set translation value. 00190 * 00191 This array denotes the translation vector of the transform. The 00192 element indexes are ordered x, y, z, i.e. translation[0] is the 00193 X value of the translation vector. 00194 00195 * @param new_translation new translation value 00196 */ 00197 void 00198 TransformInterface::set_translation(const double * new_translation) 00199 { 00200 memcpy(data->translation, new_translation, sizeof(double) * 3); 00201 data_changed = true; 00202 } 00203 00204 /** Set translation value at given index. 00205 * 00206 This array denotes the translation vector of the transform. The 00207 element indexes are ordered x, y, z, i.e. translation[0] is the 00208 X value of the translation vector. 00209 00210 * @param new_translation new translation value 00211 * @param index index for of the value 00212 */ 00213 void 00214 TransformInterface::set_translation(unsigned int index, const double new_translation) 00215 { 00216 if (index > 3) { 00217 throw Exception("Index value %u out of bounds (0..3)", index); 00218 } 00219 data->translation[index] = new_translation; 00220 data_changed = true; 00221 } 00222 /** Get rotation value. 00223 * 00224 This array denotes the rotation quaternion of the transform. The 00225 element indexes are ordered x, y, z, w, i.e. translation[0] is 00226 the X value of the rotation quaternion and translation[3] is the 00227 W value. 00228 00229 * @return rotation value 00230 */ 00231 double * 00232 TransformInterface::rotation() const 00233 { 00234 return data->rotation; 00235 } 00236 00237 /** Get rotation value at given index. 00238 * 00239 This array denotes the rotation quaternion of the transform. The 00240 element indexes are ordered x, y, z, w, i.e. translation[0] is 00241 the X value of the rotation quaternion and translation[3] is the 00242 W value. 00243 00244 * @param index index of value 00245 * @return rotation value 00246 * @exception Exception thrown if index is out of bounds 00247 */ 00248 double 00249 TransformInterface::rotation(unsigned int index) const 00250 { 00251 if (index > 4) { 00252 throw Exception("Index value %u out of bounds (0..4)", index); 00253 } 00254 return data->rotation[index]; 00255 } 00256 00257 /** Get maximum length of rotation value. 00258 * @return length of rotation value, can be length of the array or number of 00259 * maximum number of characters for a string 00260 */ 00261 size_t 00262 TransformInterface::maxlenof_rotation() const 00263 { 00264 return 4; 00265 } 00266 00267 /** Set rotation value. 00268 * 00269 This array denotes the rotation quaternion of the transform. The 00270 element indexes are ordered x, y, z, w, i.e. translation[0] is 00271 the X value of the rotation quaternion and translation[3] is the 00272 W value. 00273 00274 * @param new_rotation new rotation value 00275 */ 00276 void 00277 TransformInterface::set_rotation(const double * new_rotation) 00278 { 00279 memcpy(data->rotation, new_rotation, sizeof(double) * 4); 00280 data_changed = true; 00281 } 00282 00283 /** Set rotation value at given index. 00284 * 00285 This array denotes the rotation quaternion of the transform. The 00286 element indexes are ordered x, y, z, w, i.e. translation[0] is 00287 the X value of the rotation quaternion and translation[3] is the 00288 W value. 00289 00290 * @param new_rotation new rotation value 00291 * @param index index for of the value 00292 */ 00293 void 00294 TransformInterface::set_rotation(unsigned int index, const double new_rotation) 00295 { 00296 if (index > 4) { 00297 throw Exception("Index value %u out of bounds (0..4)", index); 00298 } 00299 data->rotation[index] = new_rotation; 00300 data_changed = true; 00301 } 00302 /* =========== message create =========== */ 00303 Message * 00304 TransformInterface::create_message(const char *type) const 00305 { 00306 throw UnknownTypeException("The given type '%s' does not match any known " 00307 "message type for this interface type.", type); 00308 } 00309 00310 00311 /** Copy values from other interface. 00312 * @param other other interface to copy values from 00313 */ 00314 void 00315 TransformInterface::copy_values(const Interface *other) 00316 { 00317 const TransformInterface *oi = dynamic_cast<const TransformInterface *>(other); 00318 if (oi == NULL) { 00319 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)", 00320 type(), other->type()); 00321 } 00322 memcpy(data, oi->data, sizeof(TransformInterface_data_t)); 00323 } 00324 00325 const char * 00326 TransformInterface::enum_tostring(const char *enumtype, int val) const 00327 { 00328 throw UnknownTypeException("Unknown enum type %s", enumtype); 00329 } 00330 00331 /* =========== messages =========== */ 00332 /** Check if message is valid and can be enqueued. 00333 * @param message Message to check 00334 * @return true if the message is valid, false otherwise. 00335 */ 00336 bool 00337 TransformInterface::message_valid(const Message *message) const 00338 { 00339 return false; 00340 } 00341 00342 /// @cond INTERNALS 00343 EXPORT_INTERFACE(TransformInterface) 00344 /// @endcond 00345 00346 00347 } // end namespace fawkes