EEROS  1.0.0.0
API for the EEROS Real-Time Robotics Framework
RosSubscriber.hpp
Go to the documentation of this file.
1 #ifndef ORG_EEROS_CONTROL_ROSSUBCRIBER_HPP_
2 #define ORG_EEROS_CONTROL_ROSSUBCRIBER_HPP_
3 
4 #include <ros/ros.h>
5 #include <ros/callback_queue.h>
8 #include <eeros/core/System.hpp>
9 #include <eeros/math/Matrix.hpp>
10 
11 namespace eeros {
12  namespace control {
13  template < typename TRosMsg, typename SigOutType >
14  class RosSubscriber : public Block1o<SigOutType> {
15 
16  public:
17  RosSubscriber(const std::string& topic, const uint32_t queueSize=1000, const bool callNewest=false) :
18  topic (topic),
20  {
21  ros::NodeHandle handle;
22  subscriber = handle.subscribe(topic, queueSize, &RosSubscriber::rosCallbackFct, this);
23  ROS_DEBUG_STREAM("RosBlockSubscriber, reading from topic: '" << topic << "' created.");
24  }
25 
26  virtual void rosCallbackFct(const TRosMsg& msg) = 0;
27 
28 // void rosCallbackFct(const TRosMsg& msg) {
29 //
30 // // USER DEFINED: 1.) Set timestamp for all outputs
31 // // 2.) Get the data from the message
32 // // 3.) Cast the data if necessary
33 // // 4.) Insert the data into output
34 //
35 // auto time = eeros::System::getTimeNs();
36 // this->out.getSignal().setTimestamp( time );
37 //
38 // this->out.getSignal().setValue(static_cast< TOutput >( msg.data) );
39 // }
40 
41  virtual void run() {
42  if (callNewest)
43  ros::getGlobalCallbackQueue()->callAvailable(); // calls callback fct. for all available messages.
44  else // Only newest message is processed. Older ones are discarded.
45  ros::getGlobalCallbackQueue()->callOne(); // calls callback fct. only for the oldest message
46  }
47 
48 
49  protected:
50  ros::Subscriber subscriber;
51  const std::string& topic;
52  bool callNewest;
53 
54  private:
55  };
56  }
57 }
58 
59 #endif /* ORG_EEROS_CONTROL_ROSSUBCRIBER_HPP_ */
virtual void rosCallbackFct(const TRosMsg &msg)=0
RosSubscriber(const std::string &topic, const uint32_t queueSize=1000, const bool callNewest=false)
Definition: RosSubscriber.hpp:17
bool callNewest
Definition: RosSubscriber.hpp:52
Definition: Block1o.hpp:12
Definition: Config.hpp:14
ros::Subscriber subscriber
Definition: RosSubscriber.hpp:50
virtual void run()
Definition: RosSubscriber.hpp:41
const std::string & topic
Definition: RosSubscriber.hpp:51
Definition: RosSubscriber.hpp:14