dev/fake_motor/fake_motor.cpp

Some tips on how to create a device for a new motor control board.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * 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.
 */
 
#include <yarp/os/Network.h>
#include <yarp/os/Searchable.h>
 
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/Drivers.h>
#include <yarp/dev/PolyDriver.h>
 
#include <cstdio>
 
using yarp::dev::DeviceDriver;
using yarp::dev::DriverCreatorOf;
using yarp::dev::Drivers;
using yarp::dev::IPositionControl;
using yarp::dev::PolyDriver;
using yarp::os::Network;
using yarp::os::Searchable;
 
class FakeMotor :
        public DeviceDriver,
        public IPositionControl
{
public:
    bool getAxes(int* ax) override
    {
        *ax = 2;
        printf("FakeMotor reporting %d axes present\n", *ax);
        return true;
    }
 
    bool positionMove(int j, double ref) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(ref);
        return true;
    }
 
    bool positionMove(const double* refs) override
    {
        YARP_UNUSED(refs);
        return true;
    }
 
    bool positionMove(const int n_joint, const int* joints, const double* refs) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(refs);
        return true;
    }
 
    bool relativeMove(int j, double delta) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(delta);
        return true;
    }
 
    bool relativeMove(const double* deltas) override
    {
        YARP_UNUSED(deltas);
        return true;
    }
 
    bool relativeMove(const int n_joint, const int* joints, const double* deltas) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(deltas);
        return true;
    }
 
    bool checkMotionDone(int j, bool* flag) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(flag);
        return true;
    }
 
    bool checkMotionDone(bool* flag) override
    {
        YARP_UNUSED(flag);
        return true;
    }
 
    bool checkMotionDone(const int n_joint, const int* joints, bool* flag) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(flag);
        return true;
    }
 
    bool setRefSpeed(int j, double sp) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(sp);
        return true;
    }
 
    bool setRefSpeeds(const double* spds) override
    {
        YARP_UNUSED(spds);
        return true;
    }
 
    bool setRefSpeeds(const int n_joint, const int* joints, const double* spds) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(spds);
        return true;
    }
 
    bool setRefAcceleration(int j, double acc) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(acc);
        return true;
    }
 
    bool setRefAccelerations(const double* accs) override
    {
        YARP_UNUSED(accs);
        return true;
    }
 
    bool setRefAccelerations(const int n_joint, const int* joints, const double* accs) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(accs);
        return true;
    }
 
    bool getRefSpeed(int j, double* ref) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(ref);
        return true;
    }
 
    bool getRefSpeeds(double* spds) override
    {
        YARP_UNUSED(spds);
        return true;
    }
 
    bool getRefSpeeds(const int n_joint, const int* joints, double* spds) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(spds);
        return true;
    }
 
    bool getRefAcceleration(int j, double* acc) override
    {
        YARP_UNUSED(j);
        YARP_UNUSED(acc);
        return true;
    }
 
    bool getRefAccelerations(double* accs) override
    {
        YARP_UNUSED(accs);
        return true;
    }
 
    bool getRefAccelerations(const int n_joint, const int* joints, double* accs) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        YARP_UNUSED(accs);
        return true;
    }
 
    bool stop(int j) override
    {
        YARP_UNUSED(j);
        return true;
    }
 
    bool stop() override
    {
        return true;
    }
 
    bool stop(const int n_joint, const int* joints) override
    {
        YARP_UNUSED(n_joint);
        YARP_UNUSED(joints);
        return true;
    }
 
    bool open(Searchable& config) override
    {
        YARP_UNUSED(config);
        return true;
    }
 
    bool close() override
    {
        return true;
    }
};
 
 
void testMotor(PolyDriver& driver)
{
    IPositionControl* pos;
    if (driver.view(pos)) {
        int ct = 0;
        pos->getAxes(&ct);
        printf("  number of axes is: %d\n", ct);
    } else {
        printf("  could not find IPositionControl interface\n");
    }
}
 
int main(int argc, char* argv[])
{
    YARP_UNUSED(argc);
    YARP_UNUSED(argv);
 
    yarp::os::Network yarp;
 
    Drivers::factory().add(new DriverCreatorOf<FakeMotor>("motor",
                                                          "controlboard",
                                                          "FakeMotor"));
 
    printf("============================================================\n");
    printf("check our device can be instantiated directly\n");
 
    PolyDriver direct("motor");
    if (direct.isValid()) {
        printf("Direct instantiation worked\n");
        testMotor(direct);
    } else {
        printf("Direct instantiation failed\n");
    }
    direct.close();
 
    // check our device can be wrapped in the controlboard network wrapper
    printf("\n\n");
    printf("============================================================\n");
    printf("check our device can be wrapped in controlboard\n");
 
 
    PolyDriver indirect("(device controlboard) (subdevice motor)");
    if (indirect.isValid()) {
        printf("Indirect instantiation worked\n");
    } else {
        printf("Indirect instantiation failed\n");
    }
    indirect.close();
 
    // check our device can be wrapped in the controlboard network wrapper
    // and accessed remotely
    printf("\n\n");
    printf("============================================================\n");
    printf("check our device can be accessed via remote_controlboard\n");
 
    PolyDriver server("(device controlboard) (subdevice motor) (name /server)");
    if (server.isValid()) {
        printf("Server instantiation worked\n");
 
        PolyDriver client("(device clientcontrolboard) (local /client) (remote /server)");
        if (client.isValid()) {
            printf("Client instantiation worked\n");
            testMotor(client);
        } else {
            printf("Client instantiation failed\n");
        }
        client.close();
    }
    server.close();
 
 
    return 0;
}