MIRA
FrameworkTransformer.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 by
3  * MetraLabs GmbH (MLAB), GERMANY
4  * and
5  * Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
6  * All rights reserved.
7  *
8  * Contact: info@mira-project.org
9  *
10  * Commercial Usage:
11  * Licensees holding valid commercial licenses may use this file in
12  * accordance with the commercial license agreement provided with the
13  * software or, alternatively, in accordance with the terms contained in
14  * a written agreement between you and MLAB or NICR.
15  *
16  * GNU General Public License Usage:
17  * Alternatively, this file may be used under the terms of the GNU
18  * General Public License version 3.0 as published by the Free Software
19  * Foundation and appearing in the file LICENSE.GPL3 included in the
20  * packaging of this file. Please review the following information to
21  * ensure the GNU General Public License version 3.0 requirements will be
22  * met: http://www.gnu.org/copyleft/gpl.html.
23  * Alternatively you may (at your option) use any later version of the GNU
24  * General Public License if such license has been publicly approved by
25  * MLAB and NICR (or its successors, if any).
26  *
27  * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
28  * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
29  * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
30  * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
33  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
34  * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
35  * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
36  * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
37  */
38 
48 #ifndef _MIRA_FRAMEWORKTRANSFORMER_H_
49 #define _MIRA_FRAMEWORKTRANSFORMER_H_
50 
51 #include <transform/Pose.h>
52 #include <transform/Transformer.h>
56 
57 #include <factory/TypeId.h>
58 
59 #include <fw/FrameworkExports.h>
60 #include <fw/Channel.h>
61 
62 namespace mira {
63 
65 
67 class FrameworkTransformer;
68 class Authority;
69 
71 
76 {
78 
79 public:
80  enum Type {
84  };
85 
86 protected:
87  friend class FrameworkTransformer;
88 
96  FrameworkTransformerNode(const std::string& id, Authority& authority) :
97  Base(id), mType(UNSPECIFIED), mAuthority(authority) {}
98 
99 public:
100 
101  virtual bool isEmpty() const;
102 
103  template<typename Transform, typename Filter>
104  Transform getTransform(const Time& time, Filter&& filter);
105 
106  Type getType() const;
107  void setType(Type type);
108 
109 private:
110 
111  template<typename ChannelTransformType, typename Transform, typename Filter>
112  bool getTransformData(const Time& time, Transform& oTransform, Filter&& filter);
113 
114 private:
115 
116  Type mType;
117 
118  Authority& mAuthority;
119  Channel<void> mChannel;
120 };
121 
122 template<>
123 inline std::string toString<FrameworkTransformerNode::Type>(const FrameworkTransformerNode::Type& value,
124  int precision)
125 {
126  switch (value) {
127  case FrameworkTransformerNode::UNSPECIFIED: return "UNSPECIFIED";
128  case FrameworkTransformerNode::MOVING: return "MOVING";
129  case FrameworkTransformerNode::FIXED: return "FIXED";
130  default: return "-INVALID VALUE-";
131  }
132 }
133 
135 
137 template <typename Derived>
139 {
140 public:
141 
142  template <typename Reflector>
143  void reflect(Reflector& r)
144  {
145  Derived* This = static_cast<Derived*>(this);
146 
147  r.interface("IFrameworkTransformer");
148  r.method("addLink",
149  (void (Derived::*)(const std::string&, const std::string&))&Derived::addLink,
150  This,
151  "add a link between the two specified nodes",
152  "child", "child node frame ID", "/robot/ArmFrame",
153  "parent", "parent node frame ID", "/robot/RobotBaseFrame");
154 
155  r.method("getTransform2", &Derived::getTransform2, This,
156  "obtains the 2D transform between the two specified nodes",
157  "target", "target frame ID", "/robot/RobotFrame",
158  "source", "source frame ID", "/GlobalFrame");
159 
160  r.method("getTransformCov2", &Derived::getTransformCov2, This,
161  "obtains the 2D transform with covariance between the two specified nodes",
162  "target", "target frame ID", "/robot/RobotFrame",
163  "source", "source frame ID", "/GlobalFrame");
164 
165  r.method("getTransform3", &Derived::getTransform3, This,
166  "obtains the 3D transform between the two specified nodes",
167  "target", "target frame ID", "/robot/RobotFrame",
168  "source", "source frame ID", "/GlobalFrame");
169 
170  r.method("getTransformCov3", &Derived::getTransformCov3, This,
171  "obtains the 3D transform with covariance between the two specified nodes",
172  "target", "target frame ID", "/robot/RobotFrame",
173  "source", "source frame ID", "/GlobalFrame");
174 
175  r.method("publishTransform2", &Derived::publishTransform2, This,
176  "publishes the 2D transform of the specified node",
177  "nodeID", "ID of node to change", "/robot/HandFrame",
178  "transform", "transformation for the node", Pose2());
179 
180  r.method("publishTransformCov2", &Derived::publishTransformCov2, This,
181  "publishes the 2D transform with covariance of the specified node",
182  "nodeID", "ID of node to change", "/robot/HandFrame",
183  "transform", "transformation for the node", PoseCov2());
184 
185  r.method("publishTransform3", &Derived::publishTransform3, This,
186  "publishes the 3D transform of the specified node",
187  "nodeID", "ID of node to change", "/robot/HandFrame",
188  "transform", "transformation for the node", Pose3());
189 
190  r.method("publishTransformCov3", &Derived::publishTransformCov3, This,
191  "publishes the 3D transform with covariance of the specified node",
192  "nodeID", "ID of node to change", "/robot/HandFrame",
193  "transform", "transformation for the node", PoseCov3());
194  }
195 };
196 
198 template <typename Derived>
200 {
201 public:
202 
204 
205  template <typename Reflector>
206  void reflect(Reflector& r)
207  {
209 
210  Derived* This = static_cast<Derived*>(this);
211 
212  r.interface("IFrameworkTransformerLinkType");
213  r.method("addLink",
214  (void (Derived::*)(const std::string&, const std::string&,
215  FrameworkTransformerNode::Type))&Derived::addLink,
216  This,
217  "add a link between the two specified nodes, with a specified type",
218  "child", "child node frame ID", "/robot/ArmFrame",
219  "parent", "parent node frame ID", "/robot/RobotBaseFrame",
220  "type", "Type of the node", FrameworkTransformerNode::MOVING);
221 
222  r.method("addTypedLink",
223  (void (Derived::*)(const std::string&, const std::string&,
224  FrameworkTransformerNode::Type))&Derived::addLink,
225  This,
226  "alias for addLink(child, parent, type), provided for backward compatibility",
227  "child", "child node frame ID", "/robot/ArmFrame",
228  "parent", "parent node frame ID", "/robot/RobotBaseFrame",
229  "type", "Type of the node", FrameworkTransformerNode::MOVING);
230  }
231 };
232 
234 
239  public GenericTransformer<FrameworkTransformerNode>,
240  public IFrameworkTransformerLinkType<FrameworkTransformer>
241 {
242 
244 
245 public:
246 
247  FrameworkTransformer(Authority& authority);
248 
250 
251 public:
260  void addLink(const std::string& childID, const std::string& parentID);
261 
275  void addLink(const std::string& childID, const std::string& parentID,
277 
284  template<typename Transform>
285  void publishTransform(const std::string& nodeID, const Transform& transform,
286  const Time& time = Time::now() );
287 
304  template<typename Transform, typename Filter = NearestNeighborInterpolator>
305  void publishTransformIndirect(const std::string& nodeID,
306  const std::string& targetID,
307  const std::string& sourceID,
308  const Transform& transform,
309  const Time& time = Time::now(),
310  Filter&& filter = NearestNeighborInterpolator());
311 
315  void storeTransformTree(const Path& filename);
316 
317 protected:
318 
319  void registerInterface(const std::string& interface,
320  const std::string& otherTransformer);
321 
322  bool createLink(const std::string& childID, const std::string& parentID,
324 
325  void notifyTransformerOfLink(const std::string& otherTransformer,
326  const std::string& childID, const std::string& parentID,
328 
329  void notifyTransformerOfLink(const std::string& otherTransformer,
330  bool implementsLinkType,
331  const std::string& childID, const std::string& parentID,
333 
334 private:
335 
337  FrameworkTransformer::NodePtr createNode(const std::string& ID);
338 
339  void setNodeType(NodePtr node, FrameworkTransformerNode::Type type);
340 
341 private:
342 
343  // for rpc calls only
345  RigidTransform2f getTransform2(const std::string& target,
346  const std::string& source);
347  RigidTransformCov2f getTransformCov2(const std::string& target,
348  const std::string& source);
349  RigidTransform3f getTransform3(const std::string& target,
350  const std::string& source);
351  RigidTransformCov3f getTransformCov3(const std::string& target,
352  const std::string& source);
353  void publishTransform2(const std::string& nodeID, const RigidTransform2f& transform);
354  void publishTransformCov2(const std::string& nodeID, const RigidTransformCov2f& transform);
355  void publishTransform3(const std::string& nodeID, const RigidTransform3f& transform);
356  void publishTransformCov3(const std::string& nodeID, const RigidTransformCov3f& transform);
357 
358 private:
359 
360  Authority& mAuthority;
361 
362  // lock node existence check + creation
363  boost::mutex mMutex;
364 };
365 
367 
369 {
370  return mChannel.isEmpty();
371 }
372 
374 {
375  return mType;
376 }
377 
379 {
380  mType = type;
381 }
382 
384 
385 // the general version
386 template <typename ChannelTransformType, typename Filter>
388  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, Filter& filter) {
389  Channel<ChannelTransformType> ch = channel_cast<ChannelTransformType>(channel);
390  oTransform = ch.get(time, filter);
391  }
392 };
393 
394 // special version for NearestNeighborInterpolator (defined in C file for compile speed)
395 template <typename ChannelTransformType>
397  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, NearestNeighborInterpolator&);
398 };
399 
400 // special version for LinearInterpolator (defined in C file for compile speed)
401 template <typename ChannelTransformType>
403  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolator&);
404 };
405 
406 // special version for LinearInterpolatorNoExtrapolation (defined in C file for compile speed)
407 template <typename ChannelTransformType>
409  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorNoExtrapolation&);
410 };
411 
412 // special version for LinearInterpolatorNearestNeighbourExtrapolator (defined in C file for compile speed)
413 template <typename ChannelTransformType>
415  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorNearestNeighbourExtrapolator&);
416 };
417 
418 // special version for LinearInterpolatorExtrapolationLimit (defined in C file for compile speed)
419 template <typename ChannelTransformType>
421  static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorExtrapolationLimit&);
422 };
423 
424 // defined in C file for compile speed
425 template <typename Transform>
427  static void set(Authority& authority, const std::string& nodeID,
428  const Transform& transform, const Time& time);
429 };
430 
432 
433 template<typename Transform, typename Filter>
434 inline Transform FrameworkTransformerNode::getTransform(const Time& time,
435  Filter&& filter)
436 {
437  Transform transform;
438 
439  // probe the type of the channel and perform a transform cast
440  // from that type to our desired transform type. If none of the supported
441  // transform types matches, throw an exception
442  // TODO can we do this better somehow??
443  if( !getTransformData<RigidTransform2f>(time,transform,filter) &&
444  !getTransformData<RigidTransform3f>(time,transform,filter) &&
445  !getTransformData<RigidTransformCov2f>(time,transform,filter) &&
446  !getTransformData<RigidTransformCov3f>(time,transform,filter))
447  MIRA_THROW(XRuntime, "Unknown transform type in channel '"
448  << mChannel.getID() << "', typename: "
449  << mChannel.getTypename() << ", typeid: " << mChannel.getTypeId());
450  return transform;
451 }
452 
453 template<typename ChannelTransformType, typename Transform, typename Filter>
454 inline bool FrameworkTransformerNode::getTransformData(const Time& time,
455  Transform& oTransform,
456  Filter&& filter)
457 {
458  int type = mChannel.getTypeId();
459  if(type>=0) {
460  // we have a type id, so use it for fast lookup
461  if(type!=typeId<ChannelTransformType>())
462  return false; // types do not match
463  // else types match
464  } else {
465  // if we trap here, the channel is yet untyped, so we need to
466  // use the typename of the channel to obtain the type
467  if(mChannel.getTypename()!=typeName<ChannelTransformType>())
468  return false; // typenames do not match
469  // else typenames match
470  }
471 
472  // if we reach here, the types match, so get the filtered transform
473  ChannelTransformType transform;
474  typedef typename boost::remove_reference<Filter>::type FilterType;
476  // ... and cast it to the desired type
477  oTransform = transform_cast<Transform>(transform);
478 
479  return true;
480 }
481 
483 
484 template<typename Transform>
485 inline void FrameworkTransformer::publishTransform(const std::string& nodeID,
486  const Transform& transform,
487  const Time& time)
488 {
489  FrameworkTransformWriteAccessor<Transform>::set(mAuthority,nodeID,transform,time);
490 }
491 
492 template<typename Transform, typename Filter>
493 inline void FrameworkTransformer::publishTransformIndirect(const std::string& nodeID,
494  const std::string& targetID,
495  const std::string& sourceID,
496  const Transform& transform,
497  const Time& time,
498  Filter&& filter)
499 {
500  NodePtr node = this->getNode(nodeID);
501  NodePtr target = this->getNode(targetID);
502  NodePtr source = this->getNode(sourceID);
503  if (!node)
504  MIRA_THROW(XRuntime, "Node " << nodeID << " does not exist");
505  if (!target)
506  MIRA_THROW(XRuntime, "Target node " << targetID << " does not exist");
507  if (!source)
508  MIRA_THROW(XRuntime, "Source node " << sourceID << " does not exist");
509  Transform inferredTransform = this->template inferTransform<Transform>(node,target,source,transform,time,filter);
510  publishTransform(nodeID,inferredTransform, time);
511 }
512 
514 
515 } // namespace
516 
517 #endif /* _MIRA_FRAMEWORKTRANSFORMER_H_ */
RigidTransform< float, 3 > Pose3
A 3D pose consisting of a translation and rotation.
Definition: Pose.h:65
Definition: FrameworkTransformer.h:75
TargetTransform transform_cast(const RigidTransform< OtherT, OtherD > &other)
Casts a RigidTransform or RigidTransformCov to a RigidTransform of a different type.
Definition: TransformCast.h:249
const std::string & getID() const
Return the channel ID, its name.
Definition: Channel.h:457
NodePtr getNode(const std::string &nodeID)
Returns a pointer to the node with the given ID or nullptr if it does not exist.
Definition: Transformer.h:528
void reflect(Reflector &r)
Definition: FrameworkTransformer.h:143
A generic transformer base class that can be used with different types of actual transformation nodes...
Definition: Transformer.h:250
Contains transform_cast operator.
Contains the Transformer class that contains everything to handle transformation trees and to compute...
1D nearest neighbor interpolator.
Definition: NearestNeighborInterpolator.h:68
specialize cv::DataType for our ImgPixel and inherit from cv::DataType<Vec>
Definition: IOService.h:67
boost::filesystem::path Path
Typedef of a Path (shorter version for boost::filesystem::path)
Definition: Path.h:69
FrameworkTransformerNode(const std::string &id, Authority &authority)
Creates a new node of the transform tree, that can be added to the corresponding FrameworkTransformer...
Definition: FrameworkTransformer.h:96
#define MIRA_REFLECT_BASE(reflector, BaseClass)
Macro that can be used to reflect the base class easily.
Definition: ReflectorInterface.h:912
1D linear interpolator.
Definition: LinearInterpolator.h:103
1D linear interpolator.
Definition: LinearInterpolator.h:181
void publishTransformIndirect(const std::string &nodeID, const std::string &targetID, const std::string &sourceID, const Transform &transform, const Time &time=Time::now(), Filter &&filter=NearestNeighborInterpolator())
Publish a transformation (nodeID) indirect by specifying a direct transform between two nodes (source...
Definition: FrameworkTransformer.h:493
Framework export macro declaration.
#define MIRA_THROW(ex, msg)
Macro for throwing an exception.
Definition: Exception.h:82
Definition: FrameworkTransformer.h:82
Typename getTypename() const
Return the typename of the channel.
Definition: Channel.h:481
Definition: FrameworkTransformer.h:238
Wrapper class for boost::posix_time::ptime for adding more functionality to it.
Definition: Time.h:418
class MIRA_FRAMEWORK_EXPORT Authority
forward declaration
Definition: RemoteConnection.h:74
Definition: FrameworkTransformer.h:426
RigidTransform< float, 2 > Pose2
A 2D pose consisting of a translation and rotation.
Definition: Pose.h:62
Definition: FrameworkTransformer.h:83
Filter that returns the nearest neighbor.
Definition: FrameworkTransformer.h:387
1D linear interpolator.
Definition: LinearInterpolator.h:70
#define MIRA_FRAMEWORK_EXPORT
Definition: FrameworkExports.h:61
RigidTransformCov< float, 3 > PoseCov3
A 3D pose with covariance consisting of a translation and rotation.
Definition: Pose.h:71
PropertyHint type(const std::string &t)
Sets the attribute "type" to the specified value.
Definition: PropertyHint.h:295
void setType(Type type)
Definition: FrameworkTransformer.h:378
Authorities act as a facade to the framework.
Definition: Authority.h:94
void publishTransform(const std::string &nodeID, const Transform &transform, const Time &time=Time::now())
Publish a transformation for a given node.
Definition: FrameworkTransformer.h:485
static void set(Authority &authority, const std::string &nodeID, const Transform &transform, const Time &time)
1D linear interpolator.
Definition: LinearInterpolator.h:137
IFrameworkTransformer< Derived > Base
Definition: FrameworkTransformer.h:203
Type
Definition: FrameworkTransformer.h:80
Transform getTransform(const Time &time, Filter &&filter)
Definition: FrameworkTransformer.h:434
Framework channel classes.
RigidTransformCov< float, 2 > PoseCov2
A 2D pose with covariance consisting of a translation and rotation.
Definition: Pose.h:68
Stamped< T > get()
Returns the latest data from the channel by value.
Definition: Channel.h:885
static void get(Channel< void > &channel, const Time &time, ChannelTransformType &oTransform, Filter &filter)
Definition: FrameworkTransformer.h:388
Extension of IFrameworkTransformer supporting link types.
Definition: FrameworkTransformer.h:199
static Time now() static Time eternity()
Returns the current utc based time.
Definition: Time.h:481
int getTypeId() const
Returns the type id of the channel.
Definition: Channel.h:467
PropertyHint precision(int p)
Sets the attribute "precision".
Definition: PropertyHint.h:285
virtual bool isEmpty() const
Returns if the node has data or is empty.
Definition: FrameworkTransformer.h:368
Abstract base class where all other different types of transformation nodes must be derived from...
Definition: TransformerNode.h:88
Typedefs for different Pose datatypes that are internally RigidTransforms.
Type getType() const
Definition: FrameworkTransformer.h:373
void reflect(Reflector &r)
Definition: FrameworkTransformer.h:206
Provides method for generating a unique id for any type.
Filter that can be used for linear interpolation.
bool isEmpty() const
Returns if the channel is empty (no data has been published)
Definition: Channel.h:524
IFrameworkTransformer defines RPC methods provided by FrameworkTransformer.
Definition: FrameworkTransformer.h:138
Definition: FrameworkTransformer.h:81