EEROS  1.0.0.0
API for the EEROS Real-Time Robotics Framework
RosSubscriberLaserScan.hpp
Go to the documentation of this file.
1 #ifndef ORG_EEROS_CONTROL_ROSSUBSCRIBER_LASERSCAN_HPP
2 #define ORG_EEROS_CONTROL_ROSSUBSCRIBER_LASERSCAN_HPP
3 
5 #include <eeros/core/System.hpp>
6 
7 // A-1 Include the header file of the ROS message
8 #include <sensor_msgs/LaserScan.h>
9 
10 namespace eeros {
11  namespace control {
12 
13  // C-1 Create the template definition. Each EEROS matrix output needs its own type
14  template < typename TRangesOutput, typename TIntensitiesOutput >
15  // A-3 Name your block and create the constructor. Copy the type definition
16  class RosSubscriberLaserScan : public RosSubscriber<sensor_msgs::LaserScan::Type, double> {
17  // A-2 Define the type of the ROS message
18  typedef sensor_msgs::LaserScan::Type TRosMsg;
19  public:
20  RosSubscriberLaserScan(const std::string& topic, const uint32_t queueSize=1000, const bool callNewest=false ) :
21  RosSubscriber<TRosMsg, double>(topic, queueSize, callNewest){ }
22 
23  void rosCallbackFct(const TRosMsg& msg) {
24  // B-3 Set the timestamp of all EEROS signals
25  // auto time = eeros::System::getTimeNs(); // use system time for timestamp
26  auto time = msg.header.stamp.toNSec(); // use received timestamp
34 
35  // B-4 Cast the data of the ROS message and fill it into the EEROS signals
36  angle_minOutput.getSignal().setValue( static_cast< double > ( msg.angle_min ) );
37  angle_maxOutput.getSignal().setValue( static_cast< double > ( msg.angle_max ) );
38  angle_incrementOutput.getSignal().setValue( static_cast< double > ( msg.angle_increment ) );
39  time_incrementOutput.getSignal().setValue( static_cast< double > ( msg.time_increment ) );
40  scan_timeOutput.getSignal().setValue( static_cast< double > ( msg.scan_time ) );
41  range_minOutput.getSignal().setValue( static_cast< double > ( msg.range_min ) );
42  range_maxOutput.getSignal().setValue( static_cast< double > ( msg.range_max ) );
43 
44  // C-4 Convert the vector of the ROS message to a <double>-vector
45  std::vector<double> rangesTmp( msg.ranges.begin(), msg.ranges.end() ); //cast because axes is a float32 vector
46  // C-5 Fill the vector into an EEROS matrix
47  rangesValue.setCol(0, rangesTmp);
48  // C-6 Fill the EEROS matrix in a EEROS signal
50 
51  std::vector<double> intensitiesTmp( msg.intensities.begin(), msg.intensities.end() ); //cast because axes is a float32 vector
52  intensitiesValue.setCol(0, intensitiesTmp);
54  }
55 
56  // B-2 Add a 'getOutput()' function for each output
64  // C-3 Add a 'getOutput()' function for each EEROS matrix output
67 
68  protected:
69  //TODO Header
70  // B-1 Create EEROS outputs
78  // C-2 Create a 'value' and an 'output' variable for each EEROS matrix output
79  TRangesOutput rangesValue;
81  TIntensitiesOutput intensitiesValue;
83  };
84  };
85 };
86 
87 #endif // ORG_EEROS_CONTROL_ROSSUBSCRIBER_LASERSCAN_HPP
Output< double > range_minOutput
Definition: RosSubscriberLaserScan.hpp:76
Output< TIntensitiesOutput > & getIntensitiesOutput()
Definition: RosSubscriberLaserScan.hpp:66
Output< double > range_maxOutput
Definition: RosSubscriberLaserScan.hpp:77
Output< double > & getRange_maxOutput()
Definition: RosSubscriberLaserScan.hpp:63
virtual Signal< T > & getSignal()
Definition: Output.hpp:16
Output< TRangesOutput > rangesOutput
Definition: RosSubscriberLaserScan.hpp:80
Output< double > angle_maxOutput
Definition: RosSubscriberLaserScan.hpp:72
Output< double > & getAngle_minOutput()
Definition: RosSubscriberLaserScan.hpp:57
Output< TRangesOutput > & getRangesOutput()
Definition: RosSubscriberLaserScan.hpp:65
Definition: Config.hpp:14
RosSubscriberLaserScan(const std::string &topic, const uint32_t queueSize=1000, const bool callNewest=false)
Definition: RosSubscriberLaserScan.hpp:20
Output< double > time_incrementOutput
Definition: RosSubscriberLaserScan.hpp:74
virtual void setValue(T newValue)
Definition: Signal.hpp:53
Type
Definition: HALFeatures.hpp:14
const std::string & topic
Definition: RosSubscriber.hpp:51
Output< double > & getAngle_maxOutput()
Definition: RosSubscriberLaserScan.hpp:58
TIntensitiesOutput intensitiesValue
Definition: RosSubscriberLaserScan.hpp:81
virtual void setTimestamp(timestamp_t newTimestamp)
Definition: Signal.hpp:66
Definition: RosSubscriber.hpp:14
Output< TIntensitiesOutput > intensitiesOutput
Definition: RosSubscriberLaserScan.hpp:82
Output< double > & getScan_timeOutput()
Definition: RosSubscriberLaserScan.hpp:61
void rosCallbackFct(const TRosMsg &msg)
Definition: RosSubscriberLaserScan.hpp:23
Output< double > angle_incrementOutput
Definition: RosSubscriberLaserScan.hpp:73
Definition: RosSubscriberLaserScan.hpp:16
Output< double > & getRange_minOutput()
Definition: RosSubscriberLaserScan.hpp:62
TRangesOutput rangesValue
Definition: RosSubscriberLaserScan.hpp:79
Output< double > scan_timeOutput
Definition: RosSubscriberLaserScan.hpp:75
Output< double > & getTime_incrementOutput()
Definition: RosSubscriberLaserScan.hpp:60
Output< double > angle_minOutput
Definition: RosSubscriberLaserScan.hpp:71
Output< double > & getAngle_incrementOutput()
Definition: RosSubscriberLaserScan.hpp:59