Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * Position3DInterface.cpp - Fawkes BlackBoard Interface - Position3DInterface 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/Position3DInterface.h> 00025 00026 #include <core/exceptions/software.h> 00027 00028 #include <cstring> 00029 #include <cstdlib> 00030 00031 namespace fawkes { 00032 00033 /** @class Position3DInterface <interfaces/Position3DInterface.h> 00034 * Position3DInterface Fawkes BlackBoard Interface. 00035 * 00036 Storage for a 3D pose in Euclidean space. 00037 00038 * @ingroup FawkesInterfaces 00039 */ 00040 00041 00042 00043 /** Constructor */ 00044 Position3DInterface::Position3DInterface() : Interface() 00045 { 00046 data_size = sizeof(Position3DInterface_data_t); 00047 data_ptr = malloc(data_size); 00048 data = (Position3DInterface_data_t *)data_ptr; 00049 data_ts = (interface_data_ts_t *)data_ptr; 00050 memset(data_ptr, 0, data_size); 00051 add_fieldinfo(IFT_STRING, "frame", 32, data->frame); 00052 add_fieldinfo(IFT_INT32, "visibility_history", 1, &data->visibility_history); 00053 add_fieldinfo(IFT_DOUBLE, "rotation", 4, &data->rotation); 00054 add_fieldinfo(IFT_DOUBLE, "translation", 3, &data->translation); 00055 unsigned char tmp_hash[] = {0xd, 0x82, 0xad, 0x13, 0x3c, 0xeb, 0x96, 0x82, 0x25, 0x6f, 0x2f, 0x62, 0xd7, 0x87, 0xec, 0x5a}; 00056 set_hash(tmp_hash); 00057 } 00058 00059 /** Destructor */ 00060 Position3DInterface::~Position3DInterface() 00061 { 00062 free(data_ptr); 00063 } 00064 /* Methods */ 00065 /** Get frame value. 00066 * 00067 Reference coordinate frame for the data. 00068 00069 * @return frame value 00070 */ 00071 char * 00072 Position3DInterface::frame() const 00073 { 00074 return data->frame; 00075 } 00076 00077 /** Get maximum length of frame value. 00078 * @return length of frame value, can be length of the array or number of 00079 * maximum number of characters for a string 00080 */ 00081 size_t 00082 Position3DInterface::maxlenof_frame() const 00083 { 00084 return 32; 00085 } 00086 00087 /** Set frame value. 00088 * 00089 Reference coordinate frame for the data. 00090 00091 * @param new_frame new frame value 00092 */ 00093 void 00094 Position3DInterface::set_frame(const char * new_frame) 00095 { 00096 strncpy(data->frame, new_frame, sizeof(data->frame)); 00097 data_changed = true; 00098 } 00099 00100 /** Get visibility_history value. 00101 * 00102 The visibilitiy history indicates the number of consecutive positive or negative 00103 sightings. If the history is negative, there have been as many negative sightings 00104 (object not visible) as the absolute value of the history. A positive value denotes 00105 as many positive sightings. 0 shall only be used during the initialization of the 00106 interface or if the visibility history is not updated. 00107 00108 * @return visibility_history value 00109 */ 00110 int32_t 00111 Position3DInterface::visibility_history() const 00112 { 00113 return data->visibility_history; 00114 } 00115 00116 /** Get maximum length of visibility_history value. 00117 * @return length of visibility_history value, can be length of the array or number of 00118 * maximum number of characters for a string 00119 */ 00120 size_t 00121 Position3DInterface::maxlenof_visibility_history() const 00122 { 00123 return 1; 00124 } 00125 00126 /** Set visibility_history value. 00127 * 00128 The visibilitiy history indicates the number of consecutive positive or negative 00129 sightings. If the history is negative, there have been as many negative sightings 00130 (object not visible) as the absolute value of the history. A positive value denotes 00131 as many positive sightings. 0 shall only be used during the initialization of the 00132 interface or if the visibility history is not updated. 00133 00134 * @param new_visibility_history new visibility_history value 00135 */ 00136 void 00137 Position3DInterface::set_visibility_history(const int32_t new_visibility_history) 00138 { 00139 data->visibility_history = new_visibility_history; 00140 data_changed = true; 00141 } 00142 00143 /** Get rotation value. 00144 * 00145 Rotation quaternion relative to reference frame, ordered as (x, y, z, w). 00146 00147 * @return rotation value 00148 */ 00149 double * 00150 Position3DInterface::rotation() const 00151 { 00152 return data->rotation; 00153 } 00154 00155 /** Get rotation value at given index. 00156 * 00157 Rotation quaternion relative to reference frame, ordered as (x, y, z, w). 00158 00159 * @param index index of value 00160 * @return rotation value 00161 * @exception Exception thrown if index is out of bounds 00162 */ 00163 double 00164 Position3DInterface::rotation(unsigned int index) const 00165 { 00166 if (index > 4) { 00167 throw Exception("Index value %u out of bounds (0..4)", index); 00168 } 00169 return data->rotation[index]; 00170 } 00171 00172 /** Get maximum length of rotation value. 00173 * @return length of rotation value, can be length of the array or number of 00174 * maximum number of characters for a string 00175 */ 00176 size_t 00177 Position3DInterface::maxlenof_rotation() const 00178 { 00179 return 4; 00180 } 00181 00182 /** Set rotation value. 00183 * 00184 Rotation quaternion relative to reference frame, ordered as (x, y, z, w). 00185 00186 * @param new_rotation new rotation value 00187 */ 00188 void 00189 Position3DInterface::set_rotation(const double * new_rotation) 00190 { 00191 memcpy(data->rotation, new_rotation, sizeof(double) * 4); 00192 data_changed = true; 00193 } 00194 00195 /** Set rotation value at given index. 00196 * 00197 Rotation quaternion relative to reference frame, ordered as (x, y, z, w). 00198 00199 * @param new_rotation new rotation value 00200 * @param index index for of the value 00201 */ 00202 void 00203 Position3DInterface::set_rotation(unsigned int index, const double new_rotation) 00204 { 00205 if (index > 4) { 00206 throw Exception("Index value %u out of bounds (0..4)", index); 00207 } 00208 data->rotation[index] = new_rotation; 00209 data_changed = true; 00210 } 00211 /** Get translation value. 00212 * 00213 Translation vector from the reference frame's origin, ordered as (x, y, z). 00214 00215 * @return translation value 00216 */ 00217 double * 00218 Position3DInterface::translation() const 00219 { 00220 return data->translation; 00221 } 00222 00223 /** Get translation value at given index. 00224 * 00225 Translation vector from the reference frame's origin, ordered as (x, y, z). 00226 00227 * @param index index of value 00228 * @return translation value 00229 * @exception Exception thrown if index is out of bounds 00230 */ 00231 double 00232 Position3DInterface::translation(unsigned int index) const 00233 { 00234 if (index > 3) { 00235 throw Exception("Index value %u out of bounds (0..3)", index); 00236 } 00237 return data->translation[index]; 00238 } 00239 00240 /** Get maximum length of translation value. 00241 * @return length of translation value, can be length of the array or number of 00242 * maximum number of characters for a string 00243 */ 00244 size_t 00245 Position3DInterface::maxlenof_translation() const 00246 { 00247 return 3; 00248 } 00249 00250 /** Set translation value. 00251 * 00252 Translation vector from the reference frame's origin, ordered as (x, y, z). 00253 00254 * @param new_translation new translation value 00255 */ 00256 void 00257 Position3DInterface::set_translation(const double * new_translation) 00258 { 00259 memcpy(data->translation, new_translation, sizeof(double) * 3); 00260 data_changed = true; 00261 } 00262 00263 /** Set translation value at given index. 00264 * 00265 Translation vector from the reference frame's origin, ordered as (x, y, z). 00266 00267 * @param new_translation new translation value 00268 * @param index index for of the value 00269 */ 00270 void 00271 Position3DInterface::set_translation(unsigned int index, const double new_translation) 00272 { 00273 if (index > 3) { 00274 throw Exception("Index value %u out of bounds (0..3)", index); 00275 } 00276 data->translation[index] = new_translation; 00277 data_changed = true; 00278 } 00279 /* =========== message create =========== */ 00280 Message * 00281 Position3DInterface::create_message(const char *type) const 00282 { 00283 throw UnknownTypeException("The given type '%s' does not match any known " 00284 "message type for this interface type.", type); 00285 } 00286 00287 00288 /** Copy values from other interface. 00289 * @param other other interface to copy values from 00290 */ 00291 void 00292 Position3DInterface::copy_values(const Interface *other) 00293 { 00294 const Position3DInterface *oi = dynamic_cast<const Position3DInterface *>(other); 00295 if (oi == NULL) { 00296 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)", 00297 type(), other->type()); 00298 } 00299 memcpy(data, oi->data, sizeof(Position3DInterface_data_t)); 00300 } 00301 00302 const char * 00303 Position3DInterface::enum_tostring(const char *enumtype, int val) const 00304 { 00305 throw UnknownTypeException("Unknown enum type %s", enumtype); 00306 } 00307 00308 /* =========== messages =========== */ 00309 /** Check if message is valid and can be enqueued. 00310 * @param message Message to check 00311 * @return true if the message is valid, false otherwise. 00312 */ 00313 bool 00314 Position3DInterface::message_valid(const Message *message) const 00315 { 00316 return false; 00317 } 00318 00319 /// @cond INTERNALS 00320 EXPORT_INTERFACE(Position3DInterface) 00321 /// @endcond 00322 00323 00324 } // end namespace fawkes