YARP
Yet Another Robot Platform
Localization2DServer.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 YARP_DEV_LOCALIZATION2DSERVER_H
7 #define YARP_DEV_LOCALIZATION2DSERVER_H
8 
9 
10 #include <yarp/os/Network.h>
11 #include <yarp/os/RFModule.h>
12 #include <yarp/os/Time.h>
13 #include <yarp/os/Port.h>
14 #include <yarp/os/Stamp.h>
15 #include <yarp/os/Node.h>
16 #include <yarp/os/Publisher.h>
17 #include <yarp/os/BufferedPort.h>
18 #include <yarp/os/PeriodicThread.h>
19 #include <yarp/dev/PolyDriver.h>
23 #include <yarp/dev/OdometryData.h>
26 #include <math.h>
27 
51 {
52 protected:
53 
54  //general options
57 
58  //yarp
59  std::string m_local_name;
61  std::string m_rpcPortName;
63  std::string m_2DLocationPortName;
65  std::string m_odometryPortName;
66  std::string m_robot_frame;
67  std::string m_fixed_frame;
68 
69  //ROS
70  std::string m_child_frame_id;
71  std::string m_parent_frame_id;
75 
76  //drivers and interfaces
79 
81  double m_period;
85 
89 
90 private:
91  void publish_2DLocation_on_yarp_port();
92  void publish_odometry_on_yarp_port();
93  void publish_odometry_on_ROS_topic();
94  void publish_odometry_on_TF_topic();
95 
96 public:
98 
99 public:
100  virtual bool open(yarp::os::Searchable& prop) override;
101  virtual bool close() override;
102  virtual bool detachAll() override;
103  virtual bool attachAll(const yarp::dev::PolyDriverList &l) override;
104  virtual void run() override;
105 
107  bool initialize_ROS(yarp::os::Searchable& config);
108  virtual bool read(yarp::os::ConnectionReader& connection) override;
109 };
110 
111 #endif // YARP_DEV_LOCALIZATION2DSERVER_H
define control board standard interfaces
localization2DServer deprecated: A localization server which can be wrap multiple algorithms and devi...
bool initialize_ROS(yarp::os::Searchable &config)
yarp::dev::Nav2D::Map2DLocation m_current_position
yarp::dev::Nav2D::ILocalization2D * iLoc
yarp::os::BufferedPort< yarp::dev::Nav2D::Map2DLocation > m_2DLocationPort
virtual bool attachAll(const yarp::dev::PolyDriverList &l) override
Attach to a list of objects.
yarp::dev::PolyDriver pLoc
virtual bool close() override
Close the DeviceDriver.
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
virtual bool detachAll() override
Detach the object (you must have first called attach).
yarp::os::BufferedPort< yarp::dev::OdometryData > m_odometryPort
yarp::os::Publisher< yarp::rosmsg::nav_msgs::Odometry > m_odometry_publisher
yarp::os::Node * m_ros_node
virtual bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
yarp::os::Stamp m_loc_stamp
virtual void run() override
Loop function.
yarp::os::Publisher< yarp::rosmsg::tf2_msgs::TFMessage > m_tf_publisher
yarp::os::Stamp m_odom_stamp
yarp::dev::OdometryData m_current_odometry
virtual bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize_YARP(yarp::os::Searchable &config)
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
Interface for an object that can wrap/attach to to another.
ILocalization2D interface.
A container for a device driver.
Definition: PolyDriver.h:23
An interface for reading from a network connection.
The Node class.
Definition: Node.h:23
An abstraction for a periodic thread.
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition: PortReader.h:24
A mini-server for network communication.
Definition: Port.h:46
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