Test code for ICartesianControl::goToPoseSync() method

!test-gotoposesync.md

test-gotoposesync

Helper gist for robotology/community#131.

Running the test

Dependencies (in order)

1. iCub_SIM
2. yarprobotinterface --context simCartesianControl --config no_legs.xml
3. iKinCartesianSolver --context simCartesianControl --part left_arm

Launching the module

$ ./test-gotoposesync

User interaction

The user can ask for the Cartesian movement by sending the command g directly within the console, which is in turn attached to the module (i.e. no need for connecting a dedicated rpc port).

Upon receiving user requests, the robot hand will bounce between two Cartesian positions.

Expected console output

$> ./test-gotoposesync.exe
yarp: Port /local/left_arm/command:o active at tcp://192.168.1.85:30109/
yarp: Port /local/left_arm/state:i active at tcp://192.168.1.85:30110/
yarp: Port /local/left_arm/events:i active at tcp://192.168.1.85:30111/
yarp: Port /local/left_arm/rpc:o active at tcp://192.168.1.85:30112/
yarp: Sending output from /local/left_arm/rpc:o to /icubSim/cartesianController/left_arm/rpc:i using tcp
yarp: Sending output from /local/left_arm/command:o to /icubSim/cartesianController/left_arm/command:i
using udp
yarp: Receiving input from /icubSim/cartesianController/left_arm/state:o to /local/left_arm/state:i using udp
yarp: Receiving input from /icubSim/cartesianController/left_arm/events:o to /local/left_arm/events:i using udp
[INFO]created wrapper <cartesiancontrollerclient>. See C++ class ClientCartesianController for documentation.
Listening to terminal (type "quit" to stop module)
[INFO]running happily...
g
[INFO]going to (-0.289  0.036  0.001)...
[INFO]...target attained!
[ack]
g
[INFO]running happily...
[INFO]going to (-0.289 -0.164  0.001)...
[INFO]...target attained!
[ack]
[try 1 of 3] Trying to shut down
RFModule closing
WARNING: module attached to terminal calling exit() to quit.
You should be aware that this is not a good way to stop a module. Effects will be:
- class destructors will NOT be called
- code in the main after runModule() will NOT be executed
This happens because in your module you called attachTerminal() and we don't have a portable way to quit a module that is listening to the terminal.
At the moment the only way to have the module quit correctly is to avoid listening to terminal(i.e. do
not call attachTerminal()).
This will also make this annoying message go away.
yarp: Removing output from /local/left_arm/command:o to /icubSim/cartesianController/left_arm/command:iyarp: Removing input from /icubSim/cartesianController/left_arm/state:o to /local/left_arm/state:i
yarp: Removing input from /icubSim/cartesianController/left_arm/events:o to /local/left_arm/events:i
yarp: Removing output from /local/left_arm/rpc:o to /icubSim/cartesianController/left_arm/rpc:i
$>

CMakeLists.txt

# Copyright: (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
# Authors: Ugo Pattacini <ugo.pattacini@iit.it>
# CopyPolicy: Released under the terms of the GNU GPL v2.0.

cmake_minimum_required(VERSION 2.8.9)
project(test-gotoposesync)
find_package(YARP REQUIRED)
include_directories(${YARP_INCLUDE_DIRS})
add_executable(${PROJECT_NAME} main.cpp)
target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES})

main.cpp

/*
 * Copyright (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/

#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
#include <yarp/math/Math.h>

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

class TestGoToPoseSync : public RFModule
{
    PolyDriver driver;
    ICartesianControl *icart;
    Mutex mutex;

    Vector x_init,o_init;

public:
    bool configure(ResourceFinder &rf)
    {
        Property option;
        option.put("device","cartesiancontrollerclient");
        option.put("remote","/icubSim/cartesianController/left_arm");
        option.put("local","/local/left_arm");

        if (!driver.open(option))
        {
            yError()<<"Unable to open the driver!";
            return false;
        }

        driver.view(icart);
        icart->getPose(x_init,o_init);

        attachTerminal(); // attach keyboard input to respond()
        return true;
    }

    double getPeriod()
    {
        return 10.0;
    }

    bool updateModule()
    {
        yInfo()<<"running happily...";
        return true;
    }

    bool respond(const Bottle &cmd, Bottle &rep)
    {
        if (cmd.get(0).asVocab()==Vocab::encode("g"))
        {
            mutex.lock();
            rep.addVocab(Vocab::encode("ack"));

            Vector x,o;
            icart->getPose(x,o);
            if (norm(x-x_init)<0.1)
            {
                x=x_init;
                x[1]+=0.2;
            }
            else
                x=x_init;

            yInfo()<<"going to ("+x.toString(3,3)+")...";
            icart->goToPoseSync(x,o_init);
            mutex.unlock();
            icart->waitMotionDone();
            yInfo()<<"...target attained!";
        }
        else
            rep.addVocab(Vocab::encode("nack"));

        return true;
    }

    bool interruptModule()
    {
        LockGuard lg(mutex);
        icart->stopControl();
        return true;
    }

    bool close()
    {
        driver.close();
        return true;
    }
};

int main()
{
    Network yarp;
    if (!yarp.checkNetwork())
    {
        yError()<<"YARP not found";
        return 1;
    }

    ResourceFinder rf;
    TestGoToPoseSync mod;
    return mod.runModule(rf);
}