Homec4science
Diffusion liboncilla (biorob-cmake-utils) Browse

liboncilla/examples/anglesplot.cpp
c2bcb8d66873biorob-cmake-utils

Last Change
Raw File

anglesplot.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 <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;
static std::string lfhipscope = "/oncilla/status/nodes/angles/lfhip/";
static std::string rfhipscope = "/oncilla/status/nodes/angles/rfhip/";
static std::string lhhipscope = "/oncilla/status/nodes/angles/lhhip/";
static std::string rhhipscope = "/oncilla/status/nodes/angles/rhhip/";
static std::string lfkneescope = "/oncilla/status/nodes/angles/lfknee/";
static std::string rfkneescope = "/oncilla/status/nodes/angles/rfknee/";
static std::string lhkneescope = "/oncilla/status/nodes/angles/lhknee/";
static std::string rhkneescope = "/oncilla/status/nodes/angles/rhknee/";
static JointAnglesPtr lfhip = JointAngles::fromValue(0.0);
static JointAnglesPtr rfhip = JointAngles::fromValue(0.0);
static JointAnglesPtr lhhip = JointAngles::fromValue(0.0);
static JointAnglesPtr rhhip = JointAngles::fromValue(0.0);
static JointAnglesPtr lfknee = JointAngles::fromValue(0.0);
static JointAnglesPtr rfknee = JointAngles::fromValue(0.0);
static JointAnglesPtr lhknee = JointAngles::fromValue(0.0);
static JointAnglesPtr rhknee = JointAngles::fromValue(0.0);
static unsigned int ecount = 0;
void inputCallback(EventPtr event) {
// ++ecount;
// if (ecount > 20) {
JointAnglesPtr input = boost::static_pointer_cast<JointAngles>(
event->getData());
std::string eventstr = event->getScopePtr()->toString();
if (eventstr.compare(lfhipscope) == 0) {
lfhip = input;
} else if (eventstr.compare(rfhipscope) == 0) {
rfhip = input;
} else if (eventstr.compare(lhhipscope) == 0) {
lhhip = input;
} else if (eventstr.compare(rhhipscope) == 0) {
rhhip = input;
} else if (eventstr.compare(lfkneescope) == 0) {
lfknee = input;
} else if (eventstr.compare(rfkneescope) == 0) {
rfknee = input;
} else if (eventstr.compare(lhkneescope) == 0) {
lhknee = input;
} else if (eventstr.compare(rhkneescope) == 0) {
rhknee = input;
}
std::cout << "Angles<";
std::cout.precision(3);
std::cout << lfhip->asDouble(0) << ", ";
std::cout << rfhip->asDouble(0) << ", ";
std::cout << lhhip->asDouble(0) << ", ";
std::cout << rhhip->asDouble(0) << "><";
std::cout << lfknee->asDouble(0) << ", ";
std::cout << rfknee->asDouble(0) << ", ";
std::cout << lhknee->asDouble(0) << ", ";
std::cout << rhknee->asDouble(0) << ">";
std::cout << std::endl;
// ecount = 0;
// }
}
int main() {
/**
* 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);
LoggerFactory::getInstance().reconfigure(Logger::LEVEL_ERROR);
/**
* Listening to JointAngles
*/
ListenerPtr listener = Factory::getInstance().createListener(Scope(
"/oncilla/status/nodes/angles"), config);
listener->addHandler(HandlerPtr(new EventFunctionHandler(bind(
&inputCallback, _1))));
std::cout << "Now listening to joint angles ..." << std::endl;
while (true) {
sleep(1);
}
return EXIT_SUCCESS;
}
c4science · Help