ServerInertial.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_DEV_SERVERINERTIAL_SERVERINERTIAL_H
#define YARP_DEV_SERVERINERTIAL_SERVERINERTIAL_H
 
#include <cstdio>
 
#include <yarp/os/BufferedPort.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IGenericSensor.h>
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/Thread.h>
#include <yarp/os/Vocab.h>
#include <yarp/os/Bottle.h>
#include <yarp/dev/IPreciselyTimed.h>
#include <yarp/dev/IWrapper.h>
#include <yarp/dev/IMultipleWrapper.h>
 
// ROS state publisher
#include <yarp/os/Node.h>
#include <yarp/os/Publisher.h>
#include <yarp/rosmsg/sensor_msgs/Imu.h>
#include <yarp/rosmsg/impl/yarpRosHelper.h>
 
 
class ServerInertial :
        public yarp::dev::DeviceDriver,
        public yarp::dev::IWrapper,
        public yarp::dev::IMultipleWrapper,
        private yarp::os::Thread,
        public yarp::os::PortReader,
        public yarp::dev::IGenericSensor
{
private:
    bool spoke;
    std::string partName;
    yarp::dev::PolyDriver *IMU_polydriver;
    yarp::dev::IGenericSensor *IMU; //The inertial device
    yarp::dev::IPreciselyTimed *iTimed;
    double period;
    yarp::os::Port p;
    yarp::os::PortWriterBuffer<yarp::os::Bottle> writer;
    int prev_timestamp_counter;
    int curr_timestamp_counter;
    int trap;
    bool strict;
 
    // ROS data
    ROSTopicUsageType                                   useROS;                     // decide if open ROS topic or not
    std::string                                         frame_id;                   // name of the frame measures are referred to
    std::string                                         rosNodeName;                // name of the rosNode
    std::string                                         rosTopicName;               // name of the rosTopic
    yarp::os::Node                                      *rosNode;                   // add a ROS node
    yarp::os::NetUint32                                 rosMsgCounter;              // incremental counter in the ROS message
    yarp::os::Publisher<yarp::rosmsg::sensor_msgs::Imu> rosPublisherPort;           // Dedicated ROS topic publisher
    std::vector<yarp::os::NetFloat64>                   covariance;                 // empty matrix to store covariance data needed by ROS msg
 
    bool checkROSParams(yarp::os::Searchable &config);
    bool initialize_ROS();
    bool initialize_YARP(yarp::os::Searchable &prop);
 
public:
    ServerInertial();
    ServerInertial(const ServerInertial&) = delete;
    ServerInertial(ServerInertial&&) = delete;
    ServerInertial& operator=(const ServerInertial&) = delete;
    ServerInertial& operator=(ServerInertial&&) = delete;
    ~ServerInertial() override;
 
    bool open(yarp::os::Searchable& config) override;
 
    bool close() override;
 
    virtual bool getInertial(yarp::os::Bottle &bot);
 
    void run() override;
 
    bool read(yarp::os::ConnectionReader& connection) override;
 
    bool read(yarp::sig::Vector &out) override;
 
    bool getChannels(int *nc) override;
 
    bool calibrate(int ch, double v) override;
 
    bool attach(yarp::dev::PolyDriver *poly) override;
    bool detach() override;
 
    bool attachAll(const yarp::dev::PolyDriverList &p) override;
    bool detachAll() override;
 
private:
 
    bool ownDevices;
    yarp::dev::PolyDriver *subDeviceOwned;
 
    // Open the wrapper only, the attach method needs to be called before using it
    bool openDeferredAttach(yarp::os::Property& prop);
 
    // If a subdevice parameter is given to the wrapper, it will open it as well
    // and attach to it immediately.
    bool openAndAttachSubDevice(yarp::os::Property& prop);
};
 
 
#endif // YARP_DEV_SERVERINERTIAL_SERVERINERTIAL_H