AnalogWrapper.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef YARP_DEV_ANALOGWRAPPER_ANALOGWRAPPER_H
#define YARP_DEV_ANALOGWRAPPER_ANALOGWRAPPER_H
 
 //#include <list>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
 
#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Time.h>
#include <yarp/os/Property.h>
 
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Stamp.h>
 
#include <yarp/sig/Vector.h>
 
#include <yarp/dev/IAnalogSensor.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IMultipleWrapper.h>
#include <yarp/dev/api.h>
 
 
// ROS state publisher
#include <yarp/os/Node.h>
#include <yarp/os/Publisher.h>
#include <yarp/rosmsg/geometry_msgs/WrenchStamped.h>
#include <yarp/rosmsg/sensor_msgs/JointState.h>
#include <yarp/rosmsg/impl/yarpRosHelper.h>
 
 
#define DEFAULT_THREAD_PERIOD 20 //ms
 
class AnalogServerHandler;
class AnalogPortEntry;
 
 
class AnalogWrapper :
        public yarp::os::PeriodicThread,
        public yarp::dev::DeviceDriver,
        public yarp::dev::IMultipleWrapper
{
public:
    AnalogWrapper();
 
    AnalogWrapper(const AnalogWrapper&) = delete;
    AnalogWrapper(AnalogWrapper&&) = delete;
    AnalogWrapper& operator=(const AnalogWrapper&) = delete;
    AnalogWrapper& operator=(AnalogWrapper&&) = delete;
 
    ~AnalogWrapper() override;
 
    bool open(yarp::os::Searchable &params) override;
    bool close() override;
    yarp::os::Bottle getOptions();
 
    void setId(const std::string &id);
    std::string getId();
 
    bool attachAll(const yarp::dev::PolyDriverList &p) override;
    bool detachAll() override;
 
    void attach(yarp::dev::IAnalogSensor *s);
    void detach();
 
    bool threadInit() override;
    void threadRelease() override;
    void run() override;
 
private:
    std::string streamingPortName;
    std::string rpcPortName;
    yarp::dev::IAnalogSensor *analogSensor_p{nullptr};   // the analog sensor to read from
    std::vector<AnalogPortEntry> analogPorts;   // the list of output ports
    std::vector<AnalogServerHandler*> handlers; // the list of rpc port handlers
    yarp::os::Stamp lastStateStamp;             // the last reading time stamp
    yarp::sig::Vector lastDataRead;             // the last vector of data read from the attached IAnalogSensor
    int _rate{DEFAULT_THREAD_PERIOD};
    std::string sensorId;
 
    // ROS state publisher
    ROSTopicUsageType                                        useROS{ROS_disabled};       // decide if open ROS topic or not
    std::vector<std::string>                                 frame_idVec;                // name of the reference frame the measures are referred to
    std::vector<std::string>                                 ros_joint_names;
    std::string                                              rosMsgType;                 // ros message type
    std::string                                              rosNodeName{""};            // name of the rosNode
    std::vector<std::string>                                 rosTopicNamesVec;           // names of the rosTopics
    yarp::os::Node*                                          rosNode{nullptr};           // add a ROS node
    std::vector<yarp::os::NetUint32>                         rosMsgCounterVec;           // incremental counter in the ROS message
    int                                                      rosOffset{0};               // offset to be ignored from the analog sensor data
    int                                                      rosPadding{0};              // padding to be ignored from the analog sensor data
 
    // TODO: in the future, in order to support multiple ROS msgs this should be a pointer allocated dynamically depending on the msg maybe (??)
    //  yarp::os::PortWriterBuffer<yarp::rosmsg::geometry_msgs::WrenchStamped> rosOutputWrench_buffer; // Buffer associated to the ROS topic
    std::vector<yarp::os::Publisher<yarp::rosmsg::geometry_msgs::WrenchStamped>> rosPublisherWrenchPortVec; // Dedicated ROS topic publisher
 
    //yarp::os::PortWriterBuffer<yarp::rosmsg::sensor_msgs::JointState> rosOutputJoint_buffer; // Buffer associated to the ROS topic
    yarp::os::Publisher<yarp::rosmsg::sensor_msgs::JointState> rosPublisherJointPort; // Dedicated ROS topic publisher
 
 
    bool ownDevices{false};
    // Open the wrapper only, the attach method needs to be called before using it
    bool openDeferredAttach(yarp::os::Searchable &prop);
 
    // For the simulator, if a subdevice parameter is given to the wrapper, it will
    // open it and attach to it immediately.
    yarp::dev::PolyDriver *subDeviceOwned{nullptr};
    bool openAndAttachSubDevice(yarp::os::Searchable &prop);
 
    bool checkROSParams(yarp::os::Searchable &config);
    bool initialize_ROS();
    bool initialize_YARP(yarp::os::Searchable &config);
 
    void setHandlers();
    void removeHandlers();
 
    // Function used when there is only one output port
    bool createPort(const char* name, int rate=20);
    // Function used when one or more output ports are specified
    bool createPorts(const std::vector<AnalogPortEntry>& _analogPorts, int rate=20);
    bool checkForDeprecatedParams(yarp::os::Searchable &params);
};
 
#endif // YARP_DEV_ANALOGWRAPPER_ANALOGWRAPPER_H