Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F87288893
CCAController.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Fri, Oct 11, 19:16
Size
9 KB
Mime Type
text/x-c
Expires
Sun, Oct 13, 19:16 (1 d, 21 h)
Engine
blob
Format
Raw Data
Handle
21568565
Attached To
R6622 liboncilla
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;
}
Event Timeline
Log In to Comment