iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-TransformManager.C
Go to the documentation of this file.
1 /*! @file
2  @author Unknown
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
37 #include <nrt/Core/Model/Manager.H>
39 
40 using nrt::TransformMessage;
41 using nrt::TransformLookupMessage;
42 using nrt::TransformManagerModule;
43 
44 namespace transformuser
45 {
46 
47 NRT_DECLARE_TRANSFORM_LOOKUP_PORT( TransformLookupPort )
48 NRT_DECLARE_TRANSFORM_UPDATE_PORT( TransformUpdatePort )
49 
50 }
51 
52 class TransformUserModule : public nrt::Module,
53  public nrt::MessagePoster<transformuser::TransformLookupPort, transformuser::TransformUpdatePort>
54 {
55 public:
56  TransformUserModule( std::string const& instanceName = "" ) :
57  nrt::Module( instanceName )
58  {
59  }
60 
61  void run()
62  {
63  if( running() )
64  {
65  TransformMessage::unique_ptr notWorldToFrameA( new TransformMessage( nrt::now(), "not_world", "some_frame" ) );
66  notWorldToFrameA->transform = Eigen::Translation3d( 0, 0, 0 );
67  transformuser::TransformUpdatePort::post( notWorldToFrameA );
68 
69  TransformMessage::unique_ptr worldParentToFrameA( new TransformMessage( nrt::now(), "world_parent", "world" ) );
70  worldParentToFrameA->transform = Eigen::Translation3d( 0, 0, 0 );
71  transformuser::TransformUpdatePort::post( worldParentToFrameA );
72 
73  TransformMessage::unique_ptr worldToFrameA( new TransformMessage( nrt::now(), "world", "frame_a" ) );
74  worldToFrameA->transform = Eigen::Translation3d( 1, 1, 0 );
75  transformuser::TransformUpdatePort::post( worldToFrameA );
76 
77  TransformMessage::unique_ptr frameAToFrameB( new TransformMessage( nrt::now(), "frame_a", "frame_b" ) );
78  frameAToFrameB->transform = Eigen::Translation3d( 1, 0, 0 );
79  transformuser::TransformUpdatePort::post( frameAToFrameB );
80 
81  TransformMessage::unique_ptr frameBToFrameC( new TransformMessage( nrt::now(), "frame_b", "frame_c" ) );
82  frameBToFrameC->transform = Eigen::Translation3d( 0, 1, 0 );
83  transformuser::TransformUpdatePort::post( frameBToFrameC );
84 
85  std::deque<TransformLookupMessage::unique_ptr> lookups;
86 
87  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "world" ) ) );
88  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_a" ) ) );
89  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_b" ) ) );
90  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_c" ) ) );
91 
92  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "world" ) ) );
93  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_a" ) ) );
94  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_b" ) ) );
95  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_c" ) ) );
96 
97  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "world" ) ) );
98  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_a" ) ) );
99  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_b" ) ) );
100  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_c" ) ) );
101 
102  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "world" ) ) );
103  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_a" ) ) );
104  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_b" ) ) );
105  lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_c" ) ) );
106 
107  for( auto lookup = lookups.begin(); lookup != lookups.end(); ++lookup )
108  {
109  const auto & transform = transformuser::TransformLookupPort::post( *lookup ).get();
110  NRT_INFO( *transform );
111  }
112  }
113  }
114 };
115 
116 int main( int argc, const char ** argv )
117 {
118  try
119  {
120  // Get a handle onto our Blackboard:
122 
123  // Set some default args, in particular, we are master by default:
124  bb.setParamVal("master", true);
125 
126  // Pass command-line args to Blackboard:
127  bb.setCommandLineArgs(argc, (const char **)argv);
128 
129  // Create some modules:
130  auto transformUserModule = bb.addModule<TransformUserModule>("transform_user");
131  transformUserModule->transformuser::TransformLookupPort::setTopic( "lookup" );
132  transformUserModule->transformuser::TransformUpdatePort::setTopic( "update" );
133 
134 
135  auto transformManagerModule = bb.addModule<TransformManagerModule>("transform_manager");
136  transformManagerModule->nrt::transformmanager::TransformLookupPort::setTopicFilter( "lookup" );
137  transformManagerModule->nrt::transformmanager::TransformUpdatePort::setTopicFilter( "update" );
138 
139  bb.launch();
140 
141 // bb.printInfo( std::cout );
142 
143  bb.wait();
144 
145  NRT_DEBUG("Execution successful.");
146  }
147  catch (...) { nrt::warnAndRethrowException(); }
148 
149  return 0;
150 }