FrameTransformGet_nwc_ros.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_DEV_FRAMETRANSFORMGETNWCROS_H
#define YARP_DEV_FRAMETRANSFORMGETNWCROS_H
 
 
#include <yarp/os/Network.h>
#include <yarp/os/Node.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Publisher.h>
#include <yarp/os/Subscriber.h>
 
#include <yarp/sig/Vector.h>
 
#include <yarp/dev/IFrameTransformStorage.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/WrapperSingle.h>
 
#include <yarp/math/FrameTransform.h>
 
#include <yarp/rosmsg/geometry_msgs/TransformStamped.h>
#include <yarp/rosmsg/tf2_msgs/TFMessage.h>
 
#include <FrameTransformContainer.h>
#include <map>
#include <mutex>
 
#define ROSNODENAME "/tfNodeGet"
#define ROSTOPICNAME_TF "/tf"
#define ROSTOPICNAME_TF_STATIC "/tf_static"
 
class FrameTransformGet_nwc_ros :
    public yarp::dev::DeviceDriver,
    public yarp::os::PeriodicThread,
    public yarp::dev::IFrameTransformStorageGet
{
public:
    FrameTransformGet_nwc_ros(double tperiod=0.010);
    ~FrameTransformGet_nwc_ros()=default;
 
    //DeviceDriver
    bool open(yarp::os::Searchable& config) override;
    bool close() override;
 
    //periodicThread
    void run() override;
 
    //IFrameTransformStorageGet interface
    bool getTransforms(std::vector<yarp::math::FrameTransform>& transforms) const override;
 
    //own
    void receiveFrameTransforms();
    void rosTransformToYARPTransform(const yarp::rosmsg::geometry_msgs::TransformStamped &input, yarp::math::FrameTransform &output, bool isStatic);
    double yarpStampFromROS(const yarp::rosmsg::TickTime& rosTime);
 
private:
    mutable std::mutex                                       m_trf_mutex;
    std::string                                              m_nodeName{ROSNODENAME};
    std::string                                              m_topic{ROSTOPICNAME_TF};
    std::string                                              m_topic_static{ROSTOPICNAME_TF_STATIC};
    double                                                   m_period{0.01};
    double                                                   m_refreshInterval{0.1};
    yarp::os::Node*                                          m_rosNode{nullptr};
    yarp::os::Subscriber<yarp::rosmsg::tf2_msgs::TFMessage>  m_rosSubscriberPort_tf_timed;
    yarp::os::Subscriber<yarp::rosmsg::tf2_msgs::TFMessage>  m_rosSubscriberPort_tf_static;
    FrameTransformContainer                                  m_ftContainer;
};
 
#endif // YARP_DEV_FRAMETRANSFORMGETNWCROS_H