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);
}
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;
};
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());
}
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;
}