Fawkes API  Fawkes Development Version
Position3DInterface.cpp
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