lookup_test.cpp
Go to the documentation of this file.
1 
2 
3 #include <ros/ros.h>
4 #include <ros/time.h>
5 #include <rtt/os/main.h>
6 #include <rtt/RTT.hpp>
7 #include <ocl/DeploymentComponent.hpp>
8 #include <ocl/TaskBrowser.hpp>
9 #include "../rtt_tf-component.hpp"
10 #include <geometry_msgs/TransformStamped.h>
11 
13 public:
15  <geometry_msgs::TransformStamped(const std::string &, const std::string &, const ros::Time &)> lookup_;
17  <geometry_msgs::TransformStamped(const std::string &, const std::string &)> lookup_now_;
18  geometry_msgs::TransformStamped tform_, tform_now_;
19 
20  LookupComponent(const std::string &name) :
22  ,lookup_("lookupTransformAtTime")
23  ,lookup_now_("lookupTransform")
24  {
25  this->addProperty("tform",tform_);
26  this->addProperty("tform_now",tform_now_);
27  this->requires("tf")->addOperationCaller(lookup_);
28  this->requires("tf")->addOperationCaller(lookup_now_);
29  }
30  virtual ~LookupComponent() { }
31  bool configureHook() {
32  return true;
33  }
34  bool startHook() {
35  return true;
36  }
37  void updateHook() {
38  try{
39  tform_ = lookup_("/world","/rtt_tf_test",ros::Time::now()-ros::Duration(0.1));
40  tform_now_ = lookup_now_("/world","rel_rtt_tf_test");
41  } catch(...) {
42 
43  }
44  }
45  void stopHook() { }
46 };
47 
49 
50 int ORO_main(int argc, char** argv) {
51 
52  LookupComponent lookup("lookup");
53 
54  {
55  // Create deployer
56  OCL::DeploymentComponent deployer;
57 
58  // initialize ROS and load typekits
59  deployer.import("rtt_rosnode");
60  deployer.import("rtt_std_msgs");
61  deployer.import("rtt_geometry_msgs");
62  deployer.import("rtt_tf2_msgs");
63 
64  // import and load our component
65  deployer.import("rtt_tf");
66  deployer.loadComponent("tf","rtt_tf::RTT_TF");
67 
68  RTT::TaskContext *tf_component = deployer.myGetPeer("tf");
69 
70  // Connect components and deployer
71  deployer.connectPeers(&lookup);
72  deployer.setActivity("lookup",0.02,0,ORO_SCHED_OTHER);
73 
74  // Connect services between the tf component and the broadcaster
75  lookup.connectServices(tf_component);
76 
77  // Configure and start the components
78  tf_component->configure();
79  tf_component->start();
80 
81  lookup.configure();
82  lookup.start();
83 
84  // Create orocos taskbrowser for easy interaction
85  OCL::TaskBrowser task_browser(&deployer);
86  task_browser.loop();
87 
88  lookup.stop();
89  }
90 
91 
92  return 0;
93 }
94 
geometry_msgs::TransformStamped tform_
Definition: lookup_test.cpp:18
LookupComponent(const std::string &name)
Definition: lookup_test.cpp:20
Property< T > & addProperty(const std::string &name, T &attr)
bool setActivity(const std::string &comp_name, double period, int priority, int scheduler)
virtual bool stop()
ORO_CREATE_COMPONENT(LookupComponent)
bool import(const std::string &package)
friend friend class TaskContext
bool connectPeers(const std::string &one, const std::string &other)
virtual bool configure()
RTT::TaskContext * myGetPeer(std::string name)
bool configureHook()
Definition: lookup_test.cpp:31
geometry_msgs::TransformStamped tform_now_
Definition: lookup_test.cpp:18
ServiceRequester::shared_ptr requires()
RTT::OperationCaller< geometry_msgs::TransformStamped(const std::string &, const std::string &)> lookup_now_
Definition: lookup_test.cpp:17
int ORO_main(int argc, char **argv)
Definition: lookup_test.cpp:50
virtual ~LookupComponent()
Definition: lookup_test.cpp:30
bool loadComponent(const std::string &name, const std::string &type)
static Time now()
virtual bool connectServices(TaskContext *peer)
RTT::OperationCaller< geometry_msgs::TransformStamped(const std::string &, const std::string &, const ros::Time &)> lookup_
Definition: lookup_test.cpp:15
#define ORO_SCHED_OTHER
virtual bool start()


rtt_tf
Author(s): Ruben Smits
autogenerated on Sat Jun 8 2019 18:06:38