YARP
Yet Another Robot Platform
Localization2D_nws_ros.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_LOCALIZATION2D_NWS_ROS_H
7 #define YARP_DEV_LOCALIZATION2D_NWS_ROS_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>
20 #include <yarp/dev/WrapperSingle.h>
23 #include <yarp/dev/OdometryData.h>
26 #include <math.h>
27 
54 {
55 protected:
56 
57  //yarp
58  std::string m_local_name = "/localization2D_nws_ros";
60  std::string m_rpcPortName;
61  std::string m_robot_frame;
62  std::string m_fixed_frame;
65 
66  //ROS
67  std::string m_child_frame_id = "base_link";
68  std::string m_parent_frame_id = "odom";
69  std::string m_node_name;
70  yarp::os::Node* m_node = nullptr;
71  std::string m_odom_topic_name;
74 
75  //drivers and interfaces
78 
80  double m_period;
83 
87 
88 private:
89  void publish_odometry_on_ROS_topic();
90  void publish_odometry_on_TF_topic();
91 
92 public:
94 
95 public:
96  bool open(yarp::os::Searchable& prop) override;
97  bool close() override;
98  bool detach() override;
99  bool attach(yarp::dev::PolyDriver* driver) override;
100  void run() override;
101 
103  bool initialize_ROS(yarp::os::Searchable& config);
104  bool read(yarp::os::ConnectionReader& connection) override;
105 };
106 
107 #endif // YARP_DEV_LOCALIZATION2D_NWS_ROS
define control board standard interfaces
localization2D_nws_ros: A localization server which can be wrap multiple algorithms and devices to pr...
bool close() override
Close the DeviceDriver.
bool initialize_YARP(yarp::os::Searchable &config)
yarp::dev::OdometryData m_current_odometry
yarp::os::Publisher< yarp::rosmsg::nav_msgs::Odometry > m_odometry_publisher
yarp::dev::Nav2D::ILocalization2D * iLoc
yarp::dev::Nav2D::Map2DLocation m_current_position
yarp::dev::PolyDriver pLoc
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
yarp::os::Publisher< yarp::rosmsg::tf2_msgs::TFMessage > m_tf_publisher
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
void run() override
Loop function.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize_ROS(yarp::os::Searchable &config)
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
ILocalization2D interface.
A container for a device driver.
Definition: PolyDriver.h:23
Helper interface for an object that can wrap/or "attach" to a single other device.
Definition: WrapperSingle.h:31
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
@ localization_status_not_yet_localized