Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F78362017
anglesplot.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
Tue, Aug 20, 04:18
Size
4 KB
Mime Type
text/x-c
Expires
Thu, Aug 22, 04:18 (2 d)
Engine
blob
Format
Raw Data
Handle
20032467
Attached To
R6622 liboncilla
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;
}
Event Timeline
Log In to Comment