Homec4science
Diffusion liboncilla (biorob-cmake-utils) Browse

liboncilla/examples/oscillator.cpp
d3e446cb0f90biorob-cmake-utils

Last Change
Raw File

oscillator.cpp
View Options

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <rsc/logging/LoggerFactory.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/thread/condition.hpp>
#include <boost/timer.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/program_options.hpp>
#include <rsb/Informer.h>
#include <rsb/Handler.h>
#include <rsb/Factory.h>
#include <rsb/converter/Repository.h>
#include <rsb/converter/ProtocolBufferConverter.h>
#include <cca/Beat.h>
#include <rci/nodes/SimpleOscillator.h>
#include <rci/dto/JointAngles.h>
#include <rci/dto/converter/JointAnglesConverter.h>
using namespace std;
using namespace boost;
namespace po = boost::program_options;
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;
int main(int ac, char **av) {
double amplitude = 1.0;
double offset = 0.0;
unsigned int frequency = 1;
LoggerFactory::getInstance().reconfigure(Logger::LEVEL_ERROR);
// Declare the supported options.
po::options_description desc("Allowed options");
desc.add_options()("help", "Produce help message")("frequency",
po::value<int>(), "Frequency of oscillation in Hertz")("amplitude",
po::value<double>(), "Amplitude of the sine")("offset",
po::value<double>(), "Offset to the output value");
po::variables_map vm;
po::store(po::parse_command_line(ac, av, desc), vm);
po::notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
return 1;
}
if (vm.count("frequency")) {
frequency = vm["frequency"].as<int> ();
}
cout << "Frequency set to " << frequency << " Hertz.\n";
if (vm.count("amplitude")) {
amplitude = vm["amplitude"].as<double> ();
}
cout << "Amplitude set to " << amplitude << ".\n";
if (vm.count("offset")) {
offset = vm["offset"].as<double> ();
}
cout << "Offset set to " << offset << ".\n";
/**
* Register converters for Joint Angles
*/
boost::shared_ptr<JointAnglesConverter> converter(
new JointAnglesConverter());
stringConverterRepository()->registerConverter(converter);
// Participant configuration
rsb::ParticipantConfig config =
Factory::getInstance().getDefaultParticipantConfig();
ParticipantConfig::Transport inprocess = config.getTransport("inprocess");
inprocess.setEnabled(false);
config.addTransport(inprocess);
ParticipantConfig::Transport spread = config.getTransport("spread");
spread.setEnabled(true);
config.addTransport(spread);
/**
* Setup the main beat
*/
GlobalBeatPtr heartbeat;
heartbeat = GlobalBeat::create(50);
CCANodePtr oscillator = SimpleOscillator<JointAngles>::create(
"Knee Oscillator", frequency, amplitude, offset);
oscillator->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/lhknee"));
heartbeat->registerReceiver(oscillator);
std::cout << "Oscillating between " << (offset - amplitude) << " and "
<< (offset + amplitude) << " with " << frequency << " Hertz ..."
<< std::endl;
heartbeat->run();
return EXIT_SUCCESS;
}
c4science · Help