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