Odometry2D_nws_ros.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: LGPL-2.1-or-later
 */
 
#ifndef YARP_ODOMETRY2D_NWS_YARP_H
#define YARP_ODOMETRY2D_NWS_YARP_H
 
#include <yarp/os/Node.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Publisher.h>
#include <yarp/os/Stamp.h>
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IOdometry2D.h>
#include <yarp/dev/WrapperSingle.h>
 
#include <yarp/rosmsg/geometry_msgs/PolygonStamped.h>
#include <yarp/rosmsg/nav_msgs/Odometry.h>
#include <yarp/rosmsg/tf2_msgs/TFMessage.h>
 
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
 
#define DEG2RAD M_PI/180.0
#define DEFAULT_THREAD_PERIOD 0.02 //s
 
class Odometry2D_nws_ros :
        public yarp::os::PeriodicThread,
        public yarp::dev::DeviceDriver,
        public yarp::dev::WrapperSingle
//        public yarp::os::PortReader
{
public:
    Odometry2D_nws_ros();
    ~Odometry2D_nws_ros();
 
    // DeviceDriver
    bool open(yarp::os::Searchable &params) override;
    bool close() override;
 
    // WrapperSingle
    bool attach(yarp::dev::PolyDriver* driver) override;
    bool detach() override;
 
    // PeriodicThread
    bool threadInit() override;
    void threadRelease() override;
    void run() override;
 
 
private:
    // parameters from configuration
    std::string m_topicName;
    std::string m_nodeName;
    std::string m_odomFrame;
    std::string m_baseFrame;
    bool m_enable_publish_tf = true;
 
    // timestamp
    yarp::os::Stamp m_lastStateStamp;
 
    // period for thread
    double m_period{DEFAULT_THREAD_PERIOD};
 
    //ros node
    yarp::os::Node* m_node;
 
    //interfaces
    yarp::dev::PolyDriver m_driver;
    yarp::dev::Nav2D::IOdometry2D *m_odometry2D_interface{nullptr};
    yarp::os::Publisher<yarp::rosmsg::nav_msgs::Odometry>          rosPublisherPort_odometry;
    yarp::os::Publisher<yarp::rosmsg::tf2_msgs::TFMessage>         rosPublisherPort_tf;
 
};
 
#endif // YARP_ODOMETRY2D_NWS_YARP_H