iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
tests/test-TransformManager.C
/*! @file
@author Unknown
@copyright GNU Public License (GPL v3)
@section License
@verbatim
// ////////////////////////////////////////////////////////////////////////
// The iLab Neuromorphic Robotics Toolkit (NRT) //
// Copyright 2010-2012 by the University of Southern California (USC) //
// and the iLab at USC. //
// //
// iLab - University of Southern California //
// Hedco Neurociences Building, Room HNB-10 //
// Los Angeles, Ca 90089-2520 - USA //
// //
// See http://ilab.usc.edu for information about this project. //
// ////////////////////////////////////////////////////////////////////////
// This file is part of The iLab Neuromorphic Robotics Toolkit. //
// //
// The iLab Neuromorphic Robotics Toolkit is free software: you can //
// redistribute it and/or modify it under the terms of the GNU General //
// Public License as published by the Free Software Foundation, either //
// version 3 of the License, or (at your option) any later version. //
// //
// The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
// that it will be useful, but WITHOUT ANY WARRANTY; without even the //
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
// PURPOSE. See the GNU General Public License for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with The iLab Neuromorphic Robotics Toolkit. If not, see //
// <http://www.gnu.org/licenses/>. //
// ////////////////////////////////////////////////////////////////////////
@endverbatim */
using nrt::TransformMessage;
using nrt::TransformLookupMessage;
using nrt::TransformManagerModule;
namespace transformuser
{
NRT_DECLARE_TRANSFORM_LOOKUP_PORT( TransformLookupPort )
NRT_DECLARE_TRANSFORM_UPDATE_PORT( TransformUpdatePort )
}
class TransformUserModule : public nrt::Module,
public nrt::MessagePoster<transformuser::TransformLookupPort, transformuser::TransformUpdatePort>
{
public:
TransformUserModule( std::string const& instanceName = "" ) :
nrt::Module( instanceName )
{
}
void run()
{
if( running() )
{
TransformMessage::unique_ptr notWorldToFrameA( new TransformMessage( nrt::now(), "not_world", "some_frame" ) );
notWorldToFrameA->transform = Eigen::Translation3d( 0, 0, 0 );
transformuser::TransformUpdatePort::post( notWorldToFrameA );
TransformMessage::unique_ptr worldParentToFrameA( new TransformMessage( nrt::now(), "world_parent", "world" ) );
worldParentToFrameA->transform = Eigen::Translation3d( 0, 0, 0 );
transformuser::TransformUpdatePort::post( worldParentToFrameA );
TransformMessage::unique_ptr worldToFrameA( new TransformMessage( nrt::now(), "world", "frame_a" ) );
worldToFrameA->transform = Eigen::Translation3d( 1, 1, 0 );
transformuser::TransformUpdatePort::post( worldToFrameA );
TransformMessage::unique_ptr frameAToFrameB( new TransformMessage( nrt::now(), "frame_a", "frame_b" ) );
frameAToFrameB->transform = Eigen::Translation3d( 1, 0, 0 );
transformuser::TransformUpdatePort::post( frameAToFrameB );
TransformMessage::unique_ptr frameBToFrameC( new TransformMessage( nrt::now(), "frame_b", "frame_c" ) );
frameBToFrameC->transform = Eigen::Translation3d( 0, 1, 0 );
transformuser::TransformUpdatePort::post( frameBToFrameC );
std::deque<TransformLookupMessage::unique_ptr> lookups;
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "world" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_a" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_b" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "world", "frame_c" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "world" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_a" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_b" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_a", "frame_c" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "world" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_a" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_b" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_b", "frame_c" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "world" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_a" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_b" ) ) );
lookups.push_back( nrt::make_unique( new TransformLookupMessage( nrt::now(), "frame_c", "frame_c" ) ) );
for( auto lookup = lookups.begin(); lookup != lookups.end(); ++lookup )
{
const auto & transform = transformuser::TransformLookupPort::post( *lookup ).get();
}
}
}
};
int main( int argc, const char ** argv )
{
try
{
// Get a handle onto our Blackboard:
// Set some default args, in particular, we are master by default:
bb.setParamVal("master", true);
// Pass command-line args to Blackboard:
bb.setCommandLineArgs(argc, (const char **)argv);
// Create some modules:
auto transformUserModule = bb.addModule<TransformUserModule>("transform_user");
transformUserModule->transformuser::TransformLookupPort::setTopic( "lookup" );
transformUserModule->transformuser::TransformUpdatePort::setTopic( "update" );
auto transformManagerModule = bb.addModule<TransformManagerModule>("transform_manager");
transformManagerModule->nrt::transformmanager::TransformLookupPort::setTopicFilter( "lookup" );
transformManagerModule->nrt::transformmanager::TransformUpdatePort::setTopicFilter( "update" );
bb.launch();
// bb.printInfo( std::cout );
bb.wait();
NRT_DEBUG("Execution successful.");
}
catch (...) { nrt::warnAndRethrowException(); }
return 0;
}