Check pointing far functionality of actionsRenderingEngine

!test-are-pfar.md

test-are-pfar

A simple snippet that verifies the actionsRenderingEngine point-far functionality using the iCub simulator.

At start-up this module connects automatically to ARE and iCub_SIM. Then, the user can do:

$ yarp rpc /rpc
>> x y z

to test x y z coordinates against the pointing far.

The expected behavior is to have iCub poiting to the red ball 😉

CMakeLists.txt

# Copyright: (C) 2017 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 3.0)
project(test-pfar)

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) 2017 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/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/RpcServer.h>
#include <yarp/os/RpcClient.h>
#include <yarp/os/LogStream.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>

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

class TestModule : public RFModule
{
    RpcServer portRpc;
    RpcClient portSim;
    RpcClient portARE;

    Matrix T;

public:
    bool configure(ResourceFinder &rf)
    {
        portRpc.open("/rpc");
        portSim.open("/sim");
        portARE.open("/ARE");

        attach(portRpc);

        bool ret=true;
        ret=ret&&Network::connect("/sim","/icubSim/world");
        ret=ret&&Network::connect("/ARE","/actionsRenderingEngine/cmd:io");

        if (ret)
        {
            Bottle cmd,rep;
            cmd.addString("world");
            cmd.addString("mk");
            cmd.addString("ssph");
            cmd.addDouble(0.03);
            cmd.addDouble(0.0);
            cmd.addDouble(0.0);
            cmd.addDouble(0.0);
            cmd.addDouble(1.0);
            cmd.addDouble(0.0);
            cmd.addDouble(0.0);
            cmd.addString("false");
            portSim.write(cmd,rep);

            T=zeros(4,4);
            T(0,1)=-1.0;
            T(1,2)=1.0;  T(1,3)=0.5976;
            T(2,0)=-1.0; T(2,3)=-0.026;
            T(3,3)=1.0;
        }
        else
            yError()<<"Unable to connect to ports";

        return ret;
    }

    bool close()
    {
        portRpc.close();
        portSim.close();
        portARE.close();
        return true;
    }

    bool respond(const Bottle &cmd, Bottle &rep)
    {
        if (cmd.size()>=3)
        {
            Vector in_are(4,1.0);
            in_are[0]=cmd.get(0).asDouble();
            in_are[1]=cmd.get(1).asDouble();
            in_are[2]=cmd.get(2).asDouble();
            Vector in_world=T*in_are;

            Bottle cmd_sim,dummy1;
            cmd_sim.addString("world");
            cmd_sim.addString("set");
            cmd_sim.addString("ssph");
            cmd_sim.addInt(1);
            cmd_sim.addDouble(in_world[0]);
            cmd_sim.addDouble(in_world[1]);
            cmd_sim.addDouble(in_world[2]);
            portSim.write(cmd_sim,dummy1);

            Bottle cmd_are,dummy2;
            cmd_are.addString("pfar");
            cmd_are.addList().read(in_are.subVector(0,2));
            portARE.write(cmd_are,dummy2);

            rep.addString("ack");
        }
        else
        {
            yError()<<"wrong command received";
            rep.addString("nack");
        }
        return true;
    }

    double getPeriod()
    {
        return 1.0;
    }

    bool updateModule()
    {
        return true;
    }
};

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

    TestModule test;
    ResourceFinder rf;
    return test.runModule(rf);
}