YARP
Yet Another Robot Platform
LaserFromRosTopic.h
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: LGPL-2.1-or-later
4  */
5 
6 #ifndef LASER_FROM_ROS_TOPIC_H
7 #define LASER_FROM_ROS_TOPIC_H
8 
10 #include <yarp/os/Semaphore.h>
11 #include <yarp/os/Port.h>
12 #include <yarp/os/Subscriber.h>
13 #include <yarp/os/BufferedPort.h>
14 #include <yarp/os/Stamp.h>
17 #include <yarp/dev/LaserScan2D.h>
19 #include <yarp/sig/Vector.h>
21 #include <yarp/dev/PolyDriver.h>
22 
23  // ROS state publisher
24 #include <yarp/os/Node.h>
25 #include <yarp/os/Subscriber.h>
28 
29 #include <mutex>
30 #include <string>
31 #include <vector>
32 
33 typedef unsigned char byte;
34 
35 //---------------------------------------------------------------------------------------------------------------
37 {
40  BASE_IS_ZERO = 2
41 };
42 
43 class InputPortProcessor :
44  public yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::LaserScan>
45 {
46  std::mutex m_port_mutex;
47  yarp::dev::LaserScan2D m_lastScan;
48  yarp::os::Stamp m_lastStamp;
49  bool m_contains_data;
50 
51 public:
53  yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::LaserScan>(),
54  m_lastScan(alt.m_lastScan),
55  m_lastStamp(alt.m_lastStamp),
56  m_contains_data(alt.m_contains_data)
57  {
58  }
59 
62  virtual void onRead(yarp::rosmsg::sensor_msgs::LaserScan& v) override;
64 };
65 
74 {
75 protected:
77  std::vector <std::string> m_port_names;
79  std::vector<InputPortProcessor> m_input_ports;
80  std::vector <yarp::os::Stamp> m_last_stamp;
81  std::vector <yarp::dev::LaserScan2D> m_last_scan_data;
84 
85  std::vector <std::string> m_src_frame_id;
86  std::string m_dst_frame_id;
89 
91 
92 public:
93  LaserFromRosTopic(double period = 0.01) : Lidar2DDeviceBase(), PeriodicThread(period)
94  {
97  }
98 
100  {
101  }
102 
103  bool open(yarp::os::Searchable& config) override;
104  bool close() override;
105  bool threadInit() override;
106  void threadRelease() override;
107  void run() override;
108 
109 public:
110  //IRangefinder2D interface
111  bool setDistanceRange (double min, double max) override;
112  bool setScanLimits (double min, double max) override;
113  bool setHorizontalResolution (double step) override;
114  bool setScanRate (double rate) override;
115 
116 public:
117  //Lidar2DDeviceBase
118  bool acquireDataFromHW() override final;
119 };
120 
121 #endif
define control board standard interfaces
unsigned char byte
@ BASE_IS_NAN
@ BASE_IS_ZERO
@ BASE_IS_INF
contains the definition of a Vector type
InputPortProcessor(const InputPortProcessor &alt)
void onRead(yarp::sig::Vector &v) override
void getLast(yarp::dev::LaserScan2D &data, yarp::os::Stamp &stmp)
laserFromRosTopic: Documentation to be added
std::vector< yarp::dev::LaserScan2D > m_last_scan_data
bool close() override
Close the DeviceDriver.
std::vector< InputPortProcessor > m_input_ports
bool setDistanceRange(double min, double max) override
set the device detection range.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
std::vector< yarp::os::Stamp > m_last_stamp
void calculate(yarp::dev::LaserScan2D scan, yarp::sig::Matrix m)
std::vector< std::string > m_port_names
bool setHorizontalResolution(double step) override
get the angular step between two measurments (if available)
void run() override
Loop function.
yarp::os::Node * m_ros_node
yarp::dev::PolyDriver m_tc_driver
LaserFromRosTopic(double period=0.01)
bool setScanLimits(double min, double max) override
set the scan angular range.
bool threadInit() override
Initialization method.
yarp::sig::Vector m_empty_laser_data
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
void threadRelease() override
Release method.
std::vector< std::string > m_src_frame_id
yarp::dev::IFrameTransform * m_iTc
std::string m_dst_frame_id
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
Transform Interface.
The Lidar2DDeviceBase class.
A container for a device driver.
Definition: PolyDriver.h:23
The Node class.
Definition: Node.h:23
An abstraction for a periodic thread.
PeriodicThread(double period, ShouldUseSystemClock useSystemClock=ShouldUseSystemClock::No, PeriodicThreadClock clockAccuracy=PeriodicThreadClock::Relative)
Constructor.
void step()
Call this to "step" the thread rather than starting it.
A base class for nested structures that can be searched.
Definition: Searchable.h:63
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:21
A port specialized for reading data of a constant type published on a topic.
Definition: Subscriber.h:22
Subscriber(const std::string &name="")
Constructor.
Definition: Subscriber.h:31
A class for a Matrix.
Definition: Matrix.h:39
yarp::rosmsg::sensor_msgs::LaserScan LaserScan
Definition: LaserScan.h:21
The main, catch-all namespace for YARP.
Definition: dirs.h:16