remotecontrolboard.cpp

来自「一个语言识别引擎」· C++ 代码 · 共 2,110 行 · 第 1/5 页

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2006 Giorgio Metta
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */


#include <ace/config.h>
#include <ace/OS.h>
#include <ace/Log_Msg.h>

#include <yarp/os/PortablePair.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/Thread.h>
#include <yarp/os/Vocab.h>
#include <yarp/String.h>

#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/ControlBoardInterfacesImpl.h>

#include <yarp/sig/Vector.h>

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;

namespace yarp{
    namespace dev {
        class RemoteControlBoard;
        class ServerControlBoard;
        class CommandsHelper;
        class ImplementCallbackHelper;
    }
}

#ifndef DOXYGEN_SHOULD_SKIP_THIS

/* the control command message type 
 * head is a Bottle which contains the specification of the message type
 * body is a Vector which move the robot accordingly
 */
typedef PortablePair<Bottle, Vector> CommandMessage;

/**
 * Helper object for reading config commands for the ServerControlBoard
 * class.
 */
class yarp::dev::CommandsHelper : public PortReader {
protected:
    yarp::dev::ServerControlBoard   *caller;
    yarp::dev::IPidControl          *pid;
    yarp::dev::IPositionControl     *pos;
    yarp::dev::IVelocityControl     *vel;
    yarp::dev::IEncoders            *enc;
    yarp::dev::IAmplifierControl    *amp;
    yarp::dev::IControlLimits       *lim;
    int nj;

public:
    /**
     * Constructor.
     * @param x is the pointer to the instance of the object that uses the CommandsHelper.
     * This is required to recover the pointers to the interfaces that implement the responses
     * to the commands.
     */
    CommandsHelper(yarp::dev::ServerControlBoard *x);

    /**
     * read from the connection.
     * @param connection is a reference to a ConnectionReader object which encapsulates
     * the current port connection.
     * @return true on a successful read.
     */
    virtual bool read(ConnectionReader& connection);

    /**
     * Initialize the internal data.
     * @return true/false on success/failure
     */
    virtual bool initialize();
};


/**
 * Callback implementation after buffered input.
 */
class yarp::dev::ImplementCallbackHelper : public TypedReaderCallback<CommandMessage> {
protected:
    IPositionControl *pos;
    IVelocityControl *vel;

public:
    /**
     * Constructor.
     * @param x is the instance of the container class using the callback.
     */
    ImplementCallbackHelper(yarp::dev::ServerControlBoard *x);

    /**
     * Callback function.
     * @param v is the Vector being received.
     */
    virtual void onRead(CommandMessage& v);
};

#endif // DOXYGEN_SHOULD_SKIP_THIS

/**
 * @ingroup dev_impl_wrapper
 *
 * Implement the server side of a remote
 * control board device driver. The device contains three ports:
 * - rpc_p handling the configuration interfaces of the robot
 * - state_p streaming information about the current state of the robot
 * - control_p receiving a stream of control commands (e.g. position)
 * 
 * Missing: 
 *          torque control, NOT IMPLEMENTED
 *          calibration, tentative implementation, to be improved
 *
 */
class yarp::dev::ServerControlBoard : 
    public DeviceDriver, 
            public Thread,
            public IPidControl,
            public IPositionControl,
            public IVelocityControl,
            public IEncoders,
            public IAmplifierControl,
            public IControlLimits,
            public IControlCalibration
 // convenient to put these here just to make sure all
 // methods get implemented
{
private:
	bool spoke;
	bool verb;

    Port rpc_p;     // RPC to configure the robot
    Port state_p;   // out port to read the state
    Port control_p; // in port to command the robot

    PortWriterBuffer<yarp::sig::Vector> state_buffer;
    PortReaderBuffer<CommandMessage> control_buffer;
    yarp::dev::ImplementCallbackHelper callback_impl;

    yarp::dev::CommandsHelper command_reader;

    PolyDriver poly;
    PolyDriver polyCalib;

    int               nj;
    int               thread_period;
    IPidControl       *pid;
    IPositionControl  *pos;
    IVelocityControl  *vel;
    IEncoders         *enc;
    IAmplifierControl *amp;
    IControlLimits    *lim;
    IControlCalibration *calib;
    // LATER: other interfaces here.

public:
    /**
     * Constructor.
     */
    ServerControlBoard() : callback_impl(this), command_reader(this)
    {
        pid = NULL;
        pos = NULL;
        vel = NULL;
        enc = NULL;
        amp = NULL;
        lim = NULL;
		calib = NULL;
        nj = 0;
        thread_period = 20; // ms.
		verb = false;
    }

    /**
     * Return the value of the verbose flag.
     * @return the verbose flag.
     */
    bool verbose() const { return verb; }

    /**
     * Default open() method.
     * @return always false since initialization requires certain parameters.
     */
    virtual bool open() {
        return false;
    }
    
    /**
     * Close the device driver by deallocating all resources and closing ports.
     * @return true if successful or false otherwise.
     */
    virtual bool close() {
        if (Thread::isRunning())
            Thread::stop();

        // close the port connections here!
        rpc_p.close();
        control_p.close();
        state_p.close();

        poly.close();
        polyCalib.close();

        return true;
    }
    
    /**
     * Open the device driver.
     * @param prop is a Searchable object which contains the parameters. 
     * Allowed parameters are:
     * - verbose or v to print diagnostic information while running.
     * - subdevice to specify the name of the wrapped device.
     * - name to specify the predix of the port names.
     * - calibrator to specify the name of the calibrator object (created through a PolyDriver).
     * and all parameters required by the wrapped device driver.
     */
    virtual bool open(Searchable& prop) {
        // attach readers.
        rpc_p.setReader(command_reader);
        // attach buffers.
        state_buffer.attach(state_p);
        control_buffer.attach(control_p);
        // attach callback.
        control_buffer.useCallback(callback_impl);

		verb = (prop.check("verbose") || prop.check("v"));
		if (verb)
			ACE_OS::printf("running with verbose output\n");

        Value *name;
        if (prop.check("subdevice",name)) {
            ACE_OS::printf("Subdevice %s\n", name->toString().c_str());
            if (name->isString()) {
                // maybe user isn't doing nested configuration
                Property p;
                p.setMonitor(prop.getMonitor(),
                             "subdevice"); // pass on any monitoring
                p.fromString(prop.toString());
                p.put("device",name->toString());
                poly.open(p);
            } else {
                poly.open(*name);
            }
            if (!poly.isValid()) {
                ACE_OS::printf("cannot make <%s>\n", name->toString().c_str());
            }
        } else {
            ACE_OS::printf("\"--subdevice <name>\" not set for server_controlboard\n");
            return false;
        }

        if (prop.check("name",name)) {
            String s((size_t)1024);
            ACE_OS::sprintf(&s[0], "%s/rpc:i", name->asString().c_str());
            rpc_p.open(s.c_str());
            ACE_OS::sprintf(&s[0], "%s/command:i", name->asString().c_str());
            control_p.open(s.c_str());
            ACE_OS::sprintf(&s[0], "%s/state:o", name->asString().c_str());
            state_p.open(s.c_str());
        } else {
            rpc_p.open("/controlboard/rpc:i");
            control_p.open("/controlboard/command:i");
            state_p.open("/controlboard/state:o");
        }
        
        if (prop.check("calibrator", name))
            {
                Property p;
                p.fromString(prop.toString());
                p.put("device",name->toString());
                polyCalib.open(p);
            }

        if (poly.isValid()) {
            poly.view(pid);
            poly.view(pos);
            poly.view(vel);
            poly.view(enc);
            poly.view(amp);
            poly.view(lim);
            poly.view(calib);
        }

        // set calibrator //
        if (polyCalib.isValid())
            {
                ICalibrator *icalibrator;
                polyCalib.view(icalibrator);
                calib->setCalibrator(icalibrator);
            }
		        
        if (pid != NULL &&
            pos != NULL &&
            vel != NULL &&
            enc != NULL &&
            amp != NULL &&
            lim != NULL) {
            if (!pos->getAxes(&nj)) {
                ACE_OS::printf ("problems: controlling 0 axes\n");
                return false;
            }

            // initialization.
            command_reader.initialize();

            start();

            return true;
        }
        
        ACE_OS::printf("subdevice <%s> doesn't look like a control board (not all interfaces were acquired)\n",
                       name->toString().c_str());
        
        return false;
    }

    /**
     * The thread main loop deals with writing on ports here.
     */
    virtual void run() {
        ACE_OS::printf("Server control board starting\n");
        double before, now;
        while (!isStopping()) {
            before = Time::now();
            yarp::sig::Vector& v = state_buffer.get();
            v.size(nj);
            enc->getEncoders(&v[0]);
            // bool ok = enc->getEncoders(&v[0]);
            // LATER: deal with the ok == false.
            state_buffer.write();
            now = Time::now();
            if ((now-before)*1000 < thread_period) {
                const double k = double(thread_period)/1000.0-(now-before);
			    Time::delay(k);
            }
            else {
                ACE_OS::printf("Can't comply with the %d ms period\n", thread_period);
            }
        }
        ACE_OS::printf("Server control board stopping\n");
    }

    /* IPidControl */

    /** Set new pid value for a joint axis.
     * @param j joint number
     * @param pid new pid value
     * @return true/false on success/failure
     */
    virtual bool setPid(int j, const Pid &p) {
        if (pid)
            return pid->setPid(j, p);
        return false;
    }

    /** Set new pid value on multiple axes.
     * @param pids pointer to a vector of pids
     * @return true/false upon success/failure
     */
    virtual bool setPids(const Pid *ps) {
        if (pid)
            return pid->setPids(ps);
        return false;
    }

    /** Set the controller reference point for a given axis.
     * Warning this method can result in very large torques 
     * and should be used carefully. If you do not understand
     * this warning you should avoid using this method. 
     * Have a look at other interfaces (e.g. position control).
     * @param j joint number
     * @param ref new reference point
     * @return true/false upon success/failure
     */
    virtual bool setReference(int j, double ref) {
        if (pid)
            return pid->setReference(j, ref);
        return false;
    }

    /** Set the controller reference points, multiple axes.
     * Warning this method can result in very large torques 
     * and should be used carefully. If you do not understand
     * this warning you should avoid using this method. 
     * Have a look at other interfaces (e.g. position control).
     * @param refs pointer to the vector that contains the new reference points.
     * @return true/false upon success/failure
     */
    virtual bool setReferences(const double *refs) {
        if (pid)
            return pid->setReferences(refs);
        return false;
    }

    /** Set the error limit for the controller on a specifi joint
     * @param j joint number
     * @param limit limit value
     * @return true/false on success/failure
     */
    virtual bool setErrorLimit(int j, double limit) {
        if (pid)
            return pid->setErrorLimit(j, limit);
        return false;
    }

    /** Get the error limit for the controller on all joints.
     * @param limits pointer to the vector with the new limits
     * @return true/false on success/failure
     */
    virtual bool setErrorLimits(const double *limits) {