lookup_test.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include <ros/ros.h>
00004 #include <ros/time.h>
00005 #include <rtt/os/main.h>
00006 #include <rtt/RTT.hpp>
00007 #include <ocl/DeploymentComponent.hpp>
00008 #include <ocl/TaskBrowser.hpp>
00009 #include "../rtt_tf-component.hpp"
00010 #include <geometry_msgs/TransformStamped.h>
00011 
00012 class LookupComponent : public RTT::TaskContext {
00013 public:
00014   RTT::OperationCaller
00015     <geometry_msgs::TransformStamped(const std::string &, const std::string &, const ros::Time &)> lookup_;
00016   RTT::OperationCaller
00017     <geometry_msgs::TransformStamped(const std::string &, const std::string &)> lookup_now_;
00018   geometry_msgs::TransformStamped tform_, tform_now_;
00019 
00020   LookupComponent(const std::string &name) :
00021     RTT::TaskContext(name, RTT::TaskContext::PreOperational)
00022     ,lookup_("lookupTransformAtTime")
00023     ,lookup_now_("lookupTransform")
00024   {
00025     this->addProperty("tform",tform_);
00026     this->addProperty("tform_now",tform_now_);
00027     this->requires("tf")->addOperationCaller(lookup_);
00028     this->requires("tf")->addOperationCaller(lookup_now_);
00029   }
00030   virtual ~LookupComponent()  { }
00031   bool configureHook() { 
00032     return true;
00033   }
00034   bool startHook() {
00035     return true;
00036   }
00037   void updateHook() {
00038     try{
00039       tform_ = lookup_("/world","/rtt_tf_test",ros::Time::now()-ros::Duration(0.1));
00040       tform_now_ = lookup_now_("/world","rel_rtt_tf_test");
00041     } catch(...) {
00042       
00043     }
00044   }
00045   void stopHook() { }
00046 };
00047 
00048 ORO_CREATE_COMPONENT(LookupComponent);
00049 
00050 int ORO_main(int argc, char** argv) {
00051 
00052   LookupComponent lookup("lookup");
00053 
00054   {
00055     // Create deployer
00056     OCL::DeploymentComponent deployer;
00057 
00058     deployer.import("rtt_tf");
00059     deployer.loadComponent("tf","rtt_tf::RTT_TF");
00060 
00061     RTT::TaskContext *tf_component = deployer.myGetPeer("tf");
00062 
00063     // Connect components and deployer
00064     deployer.connectPeers(&lookup);
00065     deployer.setActivity("lookup",0.02,0,ORO_SCHED_OTHER);
00066 
00067     // Connect services between the tf component and the broadcaster
00068     lookup.connectServices(tf_component);
00069 
00070     // Configure and start the components
00071     tf_component->configure();
00072     tf_component->start();
00073 
00074     lookup.configure();
00075     lookup.start();
00076     
00077     // Create orocos taskbrowser for easy interaction
00078     OCL::TaskBrowser task_browser(&deployer);
00079     task_browser.loop();
00080 
00081     lookup.stop();
00082   }
00083 
00084 
00085   return 0;
00086 }
00087 


rtt_tf
Author(s): Ruben Smits
autogenerated on Wed Sep 16 2015 06:58:36