Robotics Service Bus

#include <iostream>

#include <fstream>

#include <stdlib.h>

#include <math.h>

#include <rsc/misc/langutils.h>

#include <rsb/converter/Repository.h>

#include <rst/converters/rci/JointAnglesConverter.h>

#include <rst/converters/rci/JointTorquesConverter.h>

#include <rst/converters/rci/JointImpedanceConverter.h>

#include <rst/converters/rci/PoseConverter.h>

#include <cca/timing/PeriodicBeat.h>

#include <cca/processing/all.h>

#include <cca/nodes/Splitter.h>

#include <cca/nodes/Collector.h>

#include <rci/dto/JointAngles.h>

#include "cca-oncilla/CCAOncilla.h"

#define TIME_STEP 10 // Webots control step in ms

using namespace std;

using namespace boost;

using namespace rci;

using namespace rci::oncilla;

using namespace rsb::converter;

using namespace rst::converters::rci;

using namespace cca;

using namespace cca::oncilla;

int main() {

    // Create a general beat for our system

    BeatPtr heartbeat = PeriodicBeat::create(TIME_STEP);

    // Instantiate the Oncilla representation in CCA

    CCAOncilla oncilla = CCAOncilla(heartbeat, PortConfiguration::CCALocal);

    // We enable the world supervisor

    SupervisorWorldPtr supervworld = oncilla.getSupervisorWorld();

    supervworld->requestTransition(ComponentState::EXECUTION());

    printf("%s", supervworld->print().c_str());

    // We enable the trunk supervisor

    SupervisorTrunkPtr supervtrunk = oncilla.getSupervisorTrunk();

    supervtrunk->configureOutputPort("out_pose",

            PortConfiguration::REMOTE("/oncilla/status/pose/trunk"));

    supervtrunk->requestTransition(ComponentState::EXECUTION());

    printf("%s", supervtrunk->print().c_str());

    // We enable the supervisors for the feet positions

    SupervisorL4Ptr supervlf = oncilla.getSupervisorL4(LEFT_FORE);

    supervlf->configureOutputPort("out_forces", PortConfiguration::REMOTE("/oncilla/supervisor/force/lf"));

    printf("%s", supervlf->print().c_str());

    SupervisorL4Ptr supervrf = oncilla.getSupervisorL4(RIGHT_FORE);

    supervrf->configureOutputPort("out_forces", PortConfiguration::REMOTE("/oncilla/supervisor/force/rf"));

    printf("%s", supervrf->print().c_str());

    SupervisorL4Ptr supervlh = oncilla.getSupervisorL4(LEFT_HIND);

    supervlh->configureOutputPort("out_forces", PortConfiguration::REMOTE("/oncilla/supervisor/force/lh"));

    printf("%s", supervlh->print().c_str());

    SupervisorL4Ptr supervrh = oncilla.getSupervisorL4(RIGHT_HIND);

    supervrh->configureOutputPort("out_forces", PortConfiguration::REMOTE("/oncilla/supervisor/force/hr"));

    printf("%s", supervrh->print().c_str());

//    SupervisorL4Ptr supervrh = oncilla.getSupervisorL4(RIGHT_HIND);

// supervrh->configureOutputPort("out_forces",

//             PortConfiguration::LOCAL("/oncilla/test2"));

//     printf("%s", supervlf->print().c_str());

    // We set up a splitter to get all eight commands and pass them to the joints

    NodePtr anglessplit = Splitter<JointAngles>::create(8);

    anglessplit->setProcessingStrategy(PortTriggered::port("in"));

    anglessplit->configureInputPort("in",

            PortConfiguration::REMOTE("/oncilla/cmd/pos/all"));

    anglessplit->configureOutputPort("out0",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l1"));

    anglessplit->configureOutputPort("out1",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l1"));

    anglessplit->configureOutputPort("out2",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l1"));

    anglessplit->configureOutputPort("out3",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l1"));

    anglessplit->configureOutputPort("out4",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l2"));

    anglessplit->configureOutputPort("out5",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l2"));

    anglessplit->configureOutputPort("out6",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l2"));

    anglessplit->configureOutputPort("out7",

            PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l2"));

    // We set up a splitter to get all eight commands and pass them to the joints

    NodePtr anglescollect = Collector<JointAngles>::create(8);

    anglescollect->setProcessingStrategy(Moderate::timeout(2));

    anglescollect->configureInputPort("in0",

            PortConfiguration::LOCAL("/oncilla/status/pos/lf/l1"));

    anglescollect->configureInputPort("in1",

            PortConfiguration::LOCAL("/oncilla/status/pos/rf/l1"));

    anglescollect->configureInputPort("in2",

            PortConfiguration::LOCAL("/oncilla/status/pos/lh/l1"));

    anglescollect->configureInputPort("in3",

            PortConfiguration::LOCAL("/oncilla/status/pos/rh/l1"));

    anglescollect->configureInputPort("in4",

            PortConfiguration::LOCAL("/oncilla/status/pos/lf/l2"));

    anglescollect->configureInputPort("in5",

            PortConfiguration::LOCAL("/oncilla/status/pos/rf/l2"));

    anglescollect->configureInputPort("in6",

            PortConfiguration::LOCAL("/oncilla/status/pos/lh/l2"));

    anglescollect->configureInputPort("in7",

            PortConfiguration::LOCAL("/oncilla/status/pos/rh/l2"));

    anglescollect->configureOutputPort("out",

            PortConfiguration::REMOTE("/oncilla/status/pos/all"));

    heartbeat->registerReceiver(anglessplit);

    heartbeat->registerReceiver(anglescollect);

    printf("*** Note:\n");

    printf("*** This example opens API Level 2 of the simulator for remote\n");

    printf("*** communication! Use the RSB logger to see what it is sending\n");

    printf("*** and start a remote application to move the robot.\n");

    heartbeat->run();

    return EXIT_SUCCESS;

}