ControlBoardWrapper.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_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H
#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H
 
 
// ControlBoardWrapper
// A modified version of the remote control board class
// which remaps joints, it can also merge networks into a single part.
 
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IMultipleWrapper.h>
 
#include <yarp/os/BufferedPort.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Stamp.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/ControlBoardHelpers.h>
 
#include <yarp/dev/impl/jointData.h> // struct for YARP extended port
 
#include <mutex>
#include <string>
#include <vector>
 
// ROS state publisher
#include <yarp/os/Node.h>
#include <yarp/os/Publisher.h>
#include <yarp/rosmsg/sensor_msgs/JointState.h>
#include <yarp/rosmsg/impl/yarpRosHelper.h>
 
 
#include "ControlBoardWrapperPidControl.h"
#include "ControlBoardWrapperPositionControl.h"
#include "ControlBoardWrapperPositionDirect.h"
#include "ControlBoardWrapperVelocityControl.h"
#include "ControlBoardWrapperEncodersTimed.h"
#include "ControlBoardWrapperMotor.h"
#include "ControlBoardWrapperMotorEncoders.h"
#include "ControlBoardWrapperAmplifierControl.h"
#include "ControlBoardWrapperControlLimits.h"
#include "ControlBoardWrapperRemoteCalibrator.h"
#include "ControlBoardWrapperControlCalibration.h"
#include "ControlBoardWrapperTorqueControl.h"
#include "ControlBoardWrapperImpedanceControl.h"
#include "ControlBoardWrapperControlMode.h"
#include "ControlBoardWrapperAxisInfo.h"
#include "ControlBoardWrapperInteractionMode.h"
#include "ControlBoardWrapperRemoteVariables.h"
#include "ControlBoardWrapperPWMControl.h"
#include "ControlBoardWrapperCurrentControl.h"
#include "ControlBoardWrapperPreciselyTimed.h"
 
#include "SubDevice.h"
#include "StreamingMessagesParser.h"
#include "RPCMessagesParser.h"
 
 
#ifdef MSVC
    #pragma warning(disable:4355)
#endif
 
/*
 * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port
 * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice.
 * (we could also use the actual joint number for each subdevice using a for loop). TODO
 */
 
class CommandsHelper;
class SubDevice;
 
 
class ControlBoardWrapper :
        public yarp::dev::DeviceDriver,
        public yarp::os::PeriodicThread,
        public yarp::dev::IMultipleWrapper,
        virtual public ControlBoardWrapperCommon,
        public ControlBoardWrapperPidControl,
        public ControlBoardWrapperPositionControl,
        public ControlBoardWrapperPositionDirect,
        public ControlBoardWrapperVelocityControl,
        public ControlBoardWrapperEncodersTimed,
        public ControlBoardWrapperMotor,
        public ControlBoardWrapperMotorEncoders,
        public ControlBoardWrapperAmplifierControl,
        public ControlBoardWrapperControlLimits,
        public ControlBoardWrapperRemoteCalibrator,
        public ControlBoardWrapperControlCalibration,
        public ControlBoardWrapperTorqueControl,
        public ControlBoardWrapperImpedanceControl,
        public ControlBoardWrapperControlMode,
        public ControlBoardWrapperAxisInfo,
        public ControlBoardWrapperPreciselyTimed,
        public ControlBoardWrapperInteractionMode,
        public ControlBoardWrapperRemoteVariables,
        public ControlBoardWrapperPWMControl,
        public ControlBoardWrapperCurrentControl
{
private:
    std::string rootName;
 
    bool checkPortName(yarp::os::Searchable &params);
 
    yarp::rosmsg::sensor_msgs::JointState ros_struct;
 
    yarp::os::BufferedPort<yarp::sig::Vector>  outputPositionStatePort;   // Port /state:o streaming out the encoder positions
    yarp::os::BufferedPort<CommandMessage>     inputStreamingPort;        // Input streaming port for high frequency commands
    yarp::os::Port inputRPCPort;                // Input RPC port for set/get remote calls
    yarp::sig::Vector times;                    // time for each joint
 
    // Buffer associated to the extendedOutputStatePort port; in this case we will use the type generated
    // from the YARP .thrift file
    yarp::os::PortWriterBuffer<yarp::dev::impl::jointData>           extendedOutputState_buffer;
    yarp::os::Port extendedOutputStatePort;         // Port /stateExt:o streaming out the struct with the robot data
 
    // ROS state publisher
    ROSTopicUsageType                                   useROS {ROS_disabled};               // decide if open ROS topic or not
    std::vector<std::string>                            jointNames;                          // name of the joints
    std::string                                         rosNodeName;                         // name of the rosNode
    std::string                                         rosTopicName;                        // name of the rosTopic
    yarp::os::Node                                      *rosNode {nullptr};                  // add a ROS node
    yarp::os::NetUint32                                 rosMsgCounter {0};                   // incremental counter in the ROS message
    yarp::os::PortWriterBuffer<yarp::rosmsg::sensor_msgs::JointState> rosOutputState_buffer; // Buffer associated to the ROS topic
    yarp::os::Publisher<yarp::rosmsg::sensor_msgs::JointState> rosPublisherPort;             // Dedicated ROS topic publisher
 
    yarp::os::PortReaderBuffer<yarp::os::Bottle>    inputRPC_buffer;                // Buffer associated to the inputRPCPort port
    RPCMessagesParser              RPC_parser;                     // Message parser associated to the inputRPCPort port
    StreamingMessagesParser        streaming_parser;               // Message parser associated to the inputStreamingPort port
 
    static constexpr double default_period = 0.02; // s
    double period {default_period};
 
    yarp::os::Bottle getOptions();
    bool updateAxisName();
    bool checkROSParams(yarp::os::Searchable &config);
    bool initialize_ROS();
    bool initialize_YARP(yarp::os::Searchable &prop);
    void cleanup_yarpPorts();
 
    // Default usage
    // Open the wrapper only, the attach method needs to be called before using it
    bool openDeferredAttach(yarp::os::Property& 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::Property& prop);
 
    bool ownDevices;
 
    void calculateMaxNumOfJointsInDevices();
 
public:
    ControlBoardWrapper();
    ControlBoardWrapper(const ControlBoardWrapper&) = delete;
    ControlBoardWrapper(ControlBoardWrapper&&) = delete;
    ControlBoardWrapper& operator=(const ControlBoardWrapper&) = delete;
    ControlBoardWrapper& operator=(ControlBoardWrapper&&) = delete;
    ~ControlBoardWrapper() override;
 
 
    /* Return id of this device */
    std::string getId()
    {
        return partName;
    }
 
    bool close() override;
 
 
    bool open(yarp::os::Searchable& prop) override;
 
    bool detachAll() override;
 
    bool attachAll(const yarp::dev::PolyDriverList &l) override;
 
    void run() override;
};
 
 
#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H