broadcaster_test.cpp
Go to the documentation of this file.
00001 
00002 #include <ros/ros.h>
00003 #include <ros/time.h>
00004 #include <rtt/os/main.h>
00005 #include <rtt/RTT.hpp>
00006 #include <ocl/DeploymentComponent.hpp>
00007 #include <ocl/TaskBrowser.hpp>
00008 #include "../rtt_tf-component.hpp"
00009 #include <geometry_msgs/TransformStamped.h>
00010 
00011 class BroadcasterComponent : public RTT::TaskContext {
00012 public:
00013   RTT::OperationCaller
00014     <void(const geometry_msgs::TransformStamped &)> broadcaster_;
00015   geometry_msgs::TransformStamped tform_;
00016 
00017   BroadcasterComponent(const std::string &name) :
00018     RTT::TaskContext(name, RTT::TaskContext::PreOperational)
00019     ,broadcaster_("broadcastTransform")
00020   {
00021     this->requires("tf")->addOperationCaller(broadcaster_);
00022   }
00023   virtual ~BroadcasterComponent()  { }
00024   bool configureHook() { 
00025     return true;
00026   }
00027   bool startHook() {
00028     tform_.transform.translation.x = 0;
00029     tform_.transform.translation.y = 0;
00030     tform_.transform.translation.z = 0;
00031     tform_.transform.rotation.x = 0;
00032     tform_.transform.rotation.y = 0;
00033     tform_.transform.rotation.z = 0;
00034     tform_.transform.rotation.w = 1;
00035 
00036     return true;
00037   }
00038   void updateHook() {
00039     RTT::os::TimeService::Seconds t = RTT::os::TimeService::Instance()->secondsSince(0);
00040     tform_.header.frame_id = "/world";
00041     tform_.header.stamp = ros::Time(((double)RTT::os::TimeService::Instance()->getNSecs())*1E-9);
00042     tform_.child_frame_id = "/rtt_tf_test";
00043     tform_.transform.translation.x = cos(t);
00044     tform_.transform.translation.y = sin(t);
00045     broadcaster_(tform_);
00046     tform_.header.frame_id = "/world";
00047     tform_.header.stamp = ros::Time(((double)RTT::os::TimeService::Instance()->getNSecs())*1E-9);
00048     tform_.child_frame_id = "rel_rtt_tf_test";
00049     tform_.transform.translation.x = cos(t);
00050     tform_.transform.translation.y = sin(t);
00051     broadcaster_(tform_);
00052   }
00053   void stopHook() { }
00054 };
00055 
00056 ORO_CREATE_COMPONENT(BroadcasterComponent);
00057 
00058 int ORO_main(int argc, char** argv) {
00059 
00060   BroadcasterComponent bcaster("bcaster");
00061 
00062   {
00063     // Create deployer
00064     OCL::DeploymentComponent deployer;
00065 
00066     deployer.import("rtt_tf");
00067     deployer.loadComponent("tf","rtt_tf::RTT_TF");
00068 
00069     RTT::TaskContext *tf_component = deployer.myGetPeer("tf");
00070 
00071     // Connect components and deployer
00072     deployer.connectPeers(&bcaster);
00073     deployer.setActivity("bcaster",0.02,0,ORO_SCHED_OTHER);
00074 
00075     // Connect services between the tf component and the broadcaster
00076     bcaster.connectServices(tf_component);
00077 
00078     // Configure and start the components
00079     tf_component->configure();
00080     tf_component->start();
00081 
00082     bcaster.configure();
00083     bcaster.start();
00084     
00085     // Create orocos taskbrowser for easy interaction
00086     OCL::TaskBrowser task_browser(&deployer);
00087     task_browser.loop();
00088   }
00089 
00090 
00091   bcaster.stop();
00092 
00093   return 0;
00094 }
00095 


rtt_tf
Author(s): Ruben Smits
autogenerated on Thu Jun 6 2019 18:07:05