Homec4science
Diffusion liboncilla (dev-arne) Browse

liboncilla/sim/webots/controller/CCAController.cpp
a51e01a7206edev-arne

Last Change
Raw File

CCAController.cpp
View Options

#include <sys/prctl.h>
#include <rsc/logging/LoggerFactory.h>
#include <rsb/Factory.h>
#include <rsb/converter/Repository.h>
#include <rsb/converter/ProtocolBufferConverter.h>
#include <cca/Beat.h>
#include <cca/processing/TimedProcessing.h>
#include <cca/processing/RestlessProcessing.h>
#include <rci/dto/JointAngles.h>
#include <rci/dto/converter/JointAnglesConverter.h>
#include <rci/dto/converter/TranslationConverter.h>
#include "libamarsi-quad/libamarsi-quad.h"
#include "WebotsSynchronizer.h"
using namespace std;
using namespace boost;
using namespace rsc;
using namespace rsc::logging;
using namespace rsb;
using namespace rsb::converter;
using namespace rst::converter::rci;
using namespace cca;
using namespace cca::rci;
using namespace cca::rci::driver;
/**
* - Interface are the Resource Nodes
* - Resource Nodes set up in main program (if possible
* - Either Synchronizer or ResourceNodes connected to Webots
* -- Resource Nodes is probably faster, but may be more
* duplicate code
* -- Synchronizer seems too much, but is all in one place and
* closer to the real hardware driver.
*/
int main(int argc, char* argv[]) {
prctl(PR_SET_DUMPABLE, 1, 0, 0, 0);
LoggerFactory::getInstance()->reconfigure(Logger::LEVEL_WARN);
/**
* Register converters for Joint Angles
*/
boost::shared_ptr<JointAnglesConverter> converter(
new JointAnglesConverter());
stringConverterRepository()->registerConverter(converter);
boost::shared_ptr<TranslationConverter> tconverter(
new TranslationConverter());
stringConverterRepository()->registerConverter(tconverter);
/**
* Setup the main beat
*/
GlobalBeatPtr heartbeat;
heartbeat = GlobalBeat::create(10);
std::cout << heartbeat->print();
std::cout << "Initializing main beat ..." << std::endl;
sleep(1);
/**
* Set up resource nodes
* Knee, hip, trunk and ankle nodes.
*/
cca::CCANodePtr lfhip, rfhip, lhhip, rhhip;
cca::CCANodePtr lfknee, rfknee, lhknee, rhknee;
cca::CCANodePtr lfankle, rfankle, lhankle, rhankle;
cca::CCANodePtr trunk;
/* Hip Nodes */
lfhip = OncillaHip::create("LEFT_FORE_HIP");
rfhip = OncillaHip::create("RIGHT_FORE_HIP");
lhhip = OncillaHip::create("LEFT_HIND_HIP");
rhhip = OncillaHip::create("RIGHT_HIND_HIP");
lfhip->setProcessingStrategy(RestlessProcessing::create());
rfhip->setProcessingStrategy(RestlessProcessing::create());
lhhip->setProcessingStrategy(RestlessProcessing::create());
rhhip->setProcessingStrategy(RestlessProcessing::create());
/* Knee Nodes */
lfknee = OncillaKnee::create("LEFT_FORE_KNEE");
rfknee = OncillaKnee::create("RIGHT_FORE_KNEE");
lhknee = OncillaKnee::create("LEFT_HIND_KNEE");
rhknee = OncillaKnee::create("RIGHT_HIND_KNEE");
lfknee->setProcessingStrategy(TimedProcessing::create());
rfknee->setProcessingStrategy(TimedProcessing::create());
lhknee->setProcessingStrategy(TimedProcessing::create());
rhknee->setProcessingStrategy(TimedProcessing::create());
/* Ankle Nodes */
lfankle = OncillaAnkle::create("LEFT_FORE_ANKLE");
rfankle = OncillaAnkle::create("RIGHT_FORE_ANKLE");
lhankle = OncillaAnkle::create("LEFT_HIND_ANKLE");
rhankle = OncillaAnkle::create("RIGHT_HIND_ANKLE");
lfankle->setProcessingStrategy(RestlessProcessing::create());
rfankle->setProcessingStrategy(RestlessProcessing::create());
lhankle->setProcessingStrategy(RestlessProcessing::create());
rhankle->setProcessingStrategy(RestlessProcessing::create());
/* Trunk Node */
trunk = OncillaTrunk::create("TRUNK");
trunk->setProcessingStrategy(RestlessProcessing::create());
/**
* Set up remote interface to Webots simulator
*/
CCANodePtr sync = WebotsSynchronizer::create(argc, argv);
sync->setProcessingStrategy(TimedProcessing::create(1));
std::cout << "Simulator created ... " << std::endl;
heartbeat->registerReceiver(sync);
heartbeat->registerReceiver(lfknee);
heartbeat->registerReceiver(rfknee);
heartbeat->registerReceiver(lhknee);
heartbeat->registerReceiver(rhknee);
std::cout << "Simulator now connected to the bus ... " << std::endl;
/**
* Connect nodes
* @todo Also via CCA?
* LEFT_FORE_HIP, //!< LEFT_FORE_HIP Actuator that drive the left fore hip joint.
* RIGHT_FORE_HIP, //!< RIGHT_FORE_HIP Actuator that drive the right fore hip joint.
* LEFT_HIND_HIP, //!< LEFT_HIND_HIP Actuator that drive the left hind hip joint.
* RIGHT_HIND_HIP, //!< RIGHT_HIND_HIP Actuator that drive the right hind hip joint.
* LEFT_FORE_KNEE, //!< LEFT_FORE_KNEE Actuator that drive the left fore knee joint.
* RIGHT_FORE_KNEE, //!< RIGHT_FORE_KNEE Actuator that drive the right fore knee joint.
* LEFT_HIND_KNEE, //!< LEFT_HIND_KNEE Actuator that drive the left hind knee joint.
* RIGHT_HIND_KNEE, //!< RIGHT_HIND_KNEE Actuator that drive the right hind knee joint.
* LEFT_FORE_TOE, //!< LEFT_FORE_TOE disabled, here for eventual improvement
* RIGHT_FORE_TOE, //!< RIGHT_FORE_TOE disabled, here for eventual improvement
* LEFT_HIND_TOE, //!< LEFT_HIND_TOE disabled, here for eventual improvement
* RIGHT_HIND_TOE, //!< RIGHT_HIND_TOE disabled, here for eventual improvement
*/
WebotsSynchronizerPtr s = static_pointer_cast<WebotsSynchronizer> (sync);
s->registerQuadNode(lfhip, amarsi::LEFT_FORE_HIP);
s->registerQuadNode(rfhip, amarsi::RIGHT_FORE_HIP);
s->registerQuadNode(lhhip, amarsi::LEFT_HIND_HIP);
s->registerQuadNode(rhhip, amarsi::RIGHT_HIND_HIP);
s->registerQuadNode(lfknee, amarsi::LEFT_FORE_KNEE);
s->registerQuadNode(rfknee, amarsi::RIGHT_FORE_KNEE);
s->registerQuadNode(lhknee, amarsi::LEFT_HIND_KNEE);
s->registerQuadNode(rhknee, amarsi::RIGHT_HIND_KNEE);
s->registerQuadNode(lfankle, amarsi::LEFT_FORE_TOE);
s->registerQuadNode(rfankle, amarsi::RIGHT_FORE_TOE);
s->registerQuadNode(lhankle, amarsi::LEFT_HIND_TOE);
s->registerQuadNode(rhankle, amarsi::RIGHT_HIND_TOE);
/**
* Connect resource nodes to the outside world
*/
std::cout << "Connecting resource nodes to the bus ... " << std::endl;
try {
// Knee inputs
lfknee->configureInputPort(
0,
PortConfiguration::REMOTE(
"/oncilla/command/nodes/angles/lfknee"));
rfknee->configureInputPort(
0,
PortConfiguration::REMOTE(
"/oncilla/command/nodes/angles/rfknee"));
lhknee->configureInputPort(
0,
PortConfiguration::REMOTE(
"/oncilla/command/nodes/angles/lhknee"));
rhknee->configureInputPort(
0,
PortConfiguration::REMOTE(
"/oncilla/command/nodes/angles/rhknee"));
// Hip inputs
lfhip->configureInputPort(
0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/lhhip"));
rfhip->configureInputPort(
0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/rfhip"));
lhhip->configureInputPort(
0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/lhhip"));
rhhip->configureInputPort(
0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/rhhip"));
// Knee outputs
lfknee->configureOutputPort(
0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/lfknee"));
rfknee->configureOutputPort(
0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/rfknee"));
lhknee->configureOutputPort(
0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/lhknee"));
rhknee->configureOutputPort(
0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/rhknee"));
// Hip outputs
lfhip->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/lfhip"));
rfhip->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/rfhip"));
lhhip->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/lhhip"));
rhhip->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/status/nodes/angles/rhhip"));
} catch (std::runtime_error e) {
std::cout << "[RemoteWebots] Caught RSB exception!" << std::endl;
std::cout << e.what() << std::endl;
sleep(10);
} catch (std::exception e) {
std::cout << "[RemoteWebots] Caught exception!" << std::endl;
std::cout << e.what() << std::endl;
sleep(10);
}
/**
* Run the whole thing
*/
boost::thread beating(bind(&GlobalBeat::run, heartbeat));
dynamic_pointer_cast<WebotsSynchronizer> (sync)->run();
beating.join();
return EXIT_SUCCESS;
}
c4science · Help