ProxyWriter(RTT::base::InputPortInterface *port): localPort(dynamic_cast<RTT::OutputPort<T> * >(port->antiClone()))
{
if(!localPort)
throw std::runtime_error("Error, could not create ProxyWriter");
RTT::TaskContext *clientTask = getClientTask();
clientTask->addPort(*localPort);
localPort->connectTo(port);
}
ProxyReader(RTT::base::OutputPortInterface *port): localPort()
{
if(!localPort)
throw std::runtime_error("Error, could not create ProxyReader");
RTT::TaskContext *clientTask = getClientTask();
clientTask->addPort(*localPort);
port->connectTo(localPort);
}
float Controller::getSurface(std::string state, std::string attr){
RTT::TaskContext* t = (TaskContext*)(getPeer(state));
if(not t){
return 0;
}
//TODO - We need to account for the case where this lookup fails
RTT::Attribute<float> a =
t->attributes()->getAttribute(attr);
return a.get();
}
RTT::OutputPort<T> &getWriter(RTT::ConnPolicy const& policy)
{
if(!writer)
{
writer = dynamic_cast<RTT::OutputPort<T> * >(port->antiClone());
RTT::TaskContext *clientTask = getClientTask();
clientTask->addPort(*writer);
port->connectTo(writer, policy);
}
return *writer;
};
RTT::InputPort<T> &getReader(RTT::ConnPolicy const& policy)
{
if(!reader)
{
reader = dynamic_cast<RTT::InputPort<T> *>(port->antiClone());
RTT::TaskContext *clientTask = getClientTask();
clientTask->addPort(*reader);
reader->connectTo(port, policy);
}
return *reader;
};
bool RTOps::configureHook() {
rtHandler.beginRT();
// Connect with the connector.
RTT::TaskContext *peer = this->getPeer("atrias_connector");
if (!peer) {
log(RTT::Error) << "[RTOps] Failed to connect to a Connector!" << RTT::endlog();
return false;
}
sendControllerOutput = peer->provides("connector")->getOperation("sendControllerOutput");
return true;
}
bool connectDefault(const std::string& robot_name) // MQueue
{
if(hasPeer(robot_name)) {
RTT::log(RTT::Info) << "Trying to connect to default joint ports."<<RTT::endlog();
RTT::TaskContext* peer = getPeer(robot_name);
if(peer == NULL) return false;
RTT::ConnPolicy policy = RTT::ConnPolicy::data();
port_JointPosition.connectTo(peer->getPort("JointPosition"),policy);
port_JointVelocity.connectTo(peer->getPort("JointVelocity"),policy);
port_JointTorque.connectTo(peer->getPort("JointTorque"),policy);
} else {
return false;
}
return port_JointPosition.connected() || port_JointVelocity.connected() || port_JointTorque.connected();
}
OrocosCommandDispatcher::OrocosCommandDispatcher(RTT::TaskContext &task, boost::function<void (int32_t actuatorId, base::actuators::DRIVE_MODE mode, double value)> setCommandCallback, ::std::string const & name, ::std::vector< boost::int32_t > const & actuatorMap): CommandDispatcher(actuatorMap, setCommandCallback), task(task)
{
std::string inputPortName("cmd_" + name);
// Verify name clashes
if (task.provides()->hasService(inputPortName))
{
RTT::log(RTT::Error) << "cannot create a dispatch called '" << inputPortName << "' as a port named '" << inputPortName << "' is already in use in the task interface" << RTT::endlog();
throw std::runtime_error(std::string("cannot create a dispatch called '") + inputPortName + "' as a port named '" + inputPortName + "' is already in use in the task interface");
}
port = new RTT::InputPort< base::actuators::Command >(inputPortName);
task.addPort(*port);
task.provides()->addEventPort(*port);
inputSample.resize(actuatorMap.size());
}
OrocsStateAggregator::OrocsStateAggregator(RTT::TaskContext& task, const std::string& name, const std::vector< int32_t >& statusMap, base::Time statusInterval) : StateAggregator(statusMap, statusInterval), task(task)
{
std::string outputPortName("status_" + name);
// Verify name clashes
if (task.provides()->hasService(outputPortName))
{
RTT::log(RTT::Error) << "cannot create a dispatch called '" << name << "' as a port named '" << outputPortName << "' is already in use in the task interface" << RTT::endlog();
throw std::runtime_error(std::string("cannot create a dispatch called '") + name + "' as a port named '" + outputPortName + "' is already in use in the task interface");
}
outputPort = new RTT::OutputPort< base::actuators::Status >(outputPortName);
task.addPort(*outputPort);
// Initialize the connections with the preallocated data structure, so that
// further write() are hard-realtime
outputPort->setDataSample(getStatus());
}
bool setupHook() {
Configuration::setStorePath("../conf");
Configuration::addPath("../conf/common");
Configuration::addPath("../conf");
Configuration::addPath("../conf/" + getRealm());
try {
// set Application echelon?
if (hasOption("level")) {
this->setEchelon(static_cast<EchelonType>(getOption<unsigned>("level")));
}
// start in simulation mode?
if (hasOption("simulation")) {
simulation = true;
// set Application echelon
if (this->getEchelon() == Echelon::None) this->setEchelon(Echelon::Sensors);
this->getMainActivity()->setPeriod(0.0);
}
// import components from this package and dependent packages
this->import(ROS_PACKAGE_NAME);
this->addTask(loadComponent("Interface", "EthernetInterface", true, Echelon::Sensors));
this->addTask(loadComponent("Navigation", "Navigation", true, Echelon::Navigation));
this->addTask(loadComponent("GPS", "GPS", false, Echelon::Sensors));
RTT::TaskContext *fmpbox = this->addTask(loadComponent("FMPBox", "Interface::FMPBox", false, Base::Sensors));
if (fmpbox) fmpbox->setActivity(new RTT::Activity(RTT::os::HighestPriority, 0, "FMPBox"));
} catch (std::runtime_error& e) {
log(Fatal) << e.what() << endlog();
return false;
}
return true;
}
int StartCommon::runCommon(state_machine::State *initialState, const std::vector< init::Base* >& toInit)
{
std::vector< init::Base* > toInitCpy(toInit);
init::TransformerBroadcaster *broadcaster = new init::TransformerBroadcaster("transformer_broadcaster", *robot);
toInitCpy.push_back(broadcaster);
init::Container all(toInitCpy);
//various init transitions
Init initializer(*transformerHelper, *configHelper, all, initialState);
state_machine::StateMachine &stateMachine(state_machine::StateMachine::getInstance());
RTT::TaskContext *clientTask = OrocosHelpers::getClientTask();
RTT::OutputPort<state_machine::serialization::Event> *eventPort = new RTT::OutputPort<state_machine::serialization::Event>();
RTT::OutputPort<state_machine::serialization::StateMachine> *dumpPort = new RTT::OutputPort<state_machine::serialization::StateMachine>();
RTT::OutputPort<std::string> *debugMessages = new RTT::OutputPort<std::string>();
clientTask->addPort("stateMachine_Events", *eventPort);
clientTask->addPort("stateMachine_Dump", *dumpPort);
clientTask->addPort("stateMachine_Msg", *debugMessages);
state_machine::serialization::StateMachine smDump(stateMachine);
if(loggingActive)
{
const std::string loggerName("taskManagement_logger");
RTT::TaskContext *logger = new logger::Logger(loggerName);
RTT::corba::TaskContextServer::Create( logger );
RTT::corba::CorbaDispatcher::Instance( logger->ports(), ORO_SCHED_OTHER, RTT::os::LowestPriority );
RTT::Activity* activity_Logger = new RTT::Activity(
ORO_SCHED_OTHER,
RTT::os::LowestPriority,
0,
logger->engine(),
"taskManagement_logger");
{ RTT::os::Thread* thread = dynamic_cast<RTT::os::Thread*>(activity_Logger);
if (thread)
thread->setStopTimeout(10);
}
logger->setActivity(activity_Logger);
orocos_cpp::LoggingHelper lHelper;
lHelper.logAllPorts(clientTask, loggerName, {}, false);
}
// if(simulationActive)
// {
//
// widget->update(smDump);
// // widget->repaint();
// app->processEvents();
// }
int cnt = 0;
stateMachine.setExecuteCallback([&](){
if(cnt >= 10)
{
cnt = 0;
dumpPort->write(smDump);
}
cnt++;
//Events for state_machine visualisation + state_machine
std::vector<state_machine::serialization::Event> newEvents = stateMachine.getNewEvents();
for(auto e: newEvents)
{
eventPort->write(e);
// if(simulationActive)
// {
// //update widget
// widget->update(e);
// // widget->repaint();
// }
}
//
// if(simulationActive)
// {
// app->processEvents();
// }
std::string debugMsgs = stateMachine.getDebugStream().str();
stateMachine.getDebugStream().str(std::string());
if(!debugMsgs.empty())
{
std::cout << debugMsgs;
debugMessages->write(debugMsgs);
}
}
);
if(loggingActive)
{
initializer.activateLogging(logExcludeList);
}
stateMachine.start(&initializer);
while (!stateMachine.execute())
{
}
return 0;
}
