diff --git a/envire.orogen b/envire.orogen index c9684d0..dfe8c8b 100644 --- a/envire.orogen +++ b/envire.orogen @@ -29,6 +29,16 @@ task_context "SynchronizationTransmitter" do argument('path', 'string') end +task_context "ConfigurableSynchronizationTransmitter" do + needs_configuration + + property "environment_path", "/std/string" + + output_port('envire_events', ro_ptr('std/vector')). + doc('envire binary event'). + keep_last_written_value(true) +end + task_context "SynchronizationReceiver" do property('export_directory', '/std/string'). doc "if set to a non-empty value, an envire scene will be saved for each received binary event. %1 can be used to add a counter to the generated path" diff --git a/tasks/ConfigurableSynchronizationTransmitter.cpp b/tasks/ConfigurableSynchronizationTransmitter.cpp new file mode 100644 index 0000000..913f695 --- /dev/null +++ b/tasks/ConfigurableSynchronizationTransmitter.cpp @@ -0,0 +1,71 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */ + +#include "ConfigurableSynchronizationTransmitter.hpp" +#include "Orocos.hpp" +#include +using namespace envire; + +ConfigurableSynchronizationTransmitter::ConfigurableSynchronizationTransmitter(std::string const& name) + : ConfigurableSynchronizationTransmitterBase(name), initial_map_sent(false),env(new envire::Environment()), envireEventDispatcher(NULL) +{ +} + +ConfigurableSynchronizationTransmitter::ConfigurableSynchronizationTransmitter(std::string const& name, RTT::ExecutionEngine* engine) + : ConfigurableSynchronizationTransmitterBase(name, engine), initial_map_sent(false), env(new envire::Environment()), envireEventDispatcher(NULL) +{ +} + +ConfigurableSynchronizationTransmitter::~ConfigurableSynchronizationTransmitter() +{ +} + +void ConfigurableSynchronizationTransmitter::loadEnvironment(const std::string &path) +{ + env.reset(envire::Environment::unserialize(path)); + envire::OrocosEmitter emitter(_envire_events); + emitter.setTime(base::Time::now()); + emitter.attach(env.get()); + emitter.flush(); +} + + +/// The following lines are template definitions for the various state machine +// hooks defined by Orocos::RTT. See ConfigurableSynchronizationTransmitter.hpp for more detailed +// documentation about them. + +bool ConfigurableSynchronizationTransmitter::configureHook() +{ + if (! ConfigurableSynchronizationTransmitterBase::configureHook()) + return false; + return true; +} +bool ConfigurableSynchronizationTransmitter::startHook() +{ + if (! ConfigurableSynchronizationTransmitterBase::startHook()) + return false; + return true; +} +void ConfigurableSynchronizationTransmitter::updateHook() +{ + ConfigurableSynchronizationTransmitterBase::updateHook(); + if(!initial_map_sent){ + LOG_ERROR_S << "Waiting"; + sleep(8); + LOG_ERROR_S << "sending"; + loadEnvironment(_environment_path.get()); + LOG_ERROR_S << "done"; + initial_map_sent = true; + } +} +void ConfigurableSynchronizationTransmitter::errorHook() +{ + ConfigurableSynchronizationTransmitterBase::errorHook(); +} +void ConfigurableSynchronizationTransmitter::stopHook() +{ + ConfigurableSynchronizationTransmitterBase::stopHook(); +} +void ConfigurableSynchronizationTransmitter::cleanupHook() +{ + ConfigurableSynchronizationTransmitterBase::cleanupHook(); +} diff --git a/tasks/ConfigurableSynchronizationTransmitter.hpp b/tasks/ConfigurableSynchronizationTransmitter.hpp new file mode 100644 index 0000000..9fab545 --- /dev/null +++ b/tasks/ConfigurableSynchronizationTransmitter.hpp @@ -0,0 +1,117 @@ +/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */ + +#ifndef ENVIRE_CONFIGURABLESYNCHRONIZATIONTRANSMITTER_TASK_HPP +#define ENVIRE_CONFIGURABLESYNCHRONIZATIONTRANSMITTER_TASK_HPP + +#include "envire/ConfigurableSynchronizationTransmitterBase.hpp" +#include + +namespace envire { + class OrocosEmitter; + + /*! \class ConfigurableSynchronizationTransmitter + * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions. + * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. + * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. + * + * \details + * The name of a TaskContext is primarily defined via: + \verbatim + deployment 'deployment_name' + task('custom_task_name','envire::ConfigurableSynchronizationTransmitter') + end + \endverbatim + * It can be dynamically adapted when the deployment is called with a prefix argument. + */ + class ConfigurableSynchronizationTransmitter : public ConfigurableSynchronizationTransmitterBase + { + friend class ConfigurableSynchronizationTransmitterBase; + protected: + boost::shared_ptr env; + envire::OrocosEmitter* envireEventDispatcher; + bool initial_map_sent; + + void loadEnvironment(const std::string &path); + + + + public: + /** TaskContext constructor for ConfigurableSynchronizationTransmitter + * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices. + * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state. + */ + ConfigurableSynchronizationTransmitter(std::string const& name = "envire::ConfigurableSynchronizationTransmitter"); + + /** TaskContext constructor for ConfigurableSynchronizationTransmitter + * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices. + * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task. + * + */ + ConfigurableSynchronizationTransmitter(std::string const& name, RTT::ExecutionEngine* engine); + + /** Default deconstructor of ConfigurableSynchronizationTransmitter + */ + ~ConfigurableSynchronizationTransmitter(); + + /** This hook is called by Orocos when the state machine transitions + * from PreOperational to Stopped. If it returns false, then the + * component will stay in PreOperational. Otherwise, it goes into + * Stopped. + * + * It is meaningful only if the #needs_configuration has been specified + * in the task context definition with (for example): + \verbatim + task_context "TaskName" do + needs_configuration + ... + end + \endverbatim + */ + bool configureHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to Running. If it returns false, then the component will + * stay in Stopped. Otherwise, it goes into Running and updateHook() + * will be called. + */ + bool startHook(); + + /** This hook is called by Orocos when the component is in the Running + * state, at each activity step. Here, the activity gives the "ticks" + * when the hook should be called. + * + * The error(), exception() and fatal() calls, when called in this hook, + * allow to get into the associated RunTimeError, Exception and + * FatalError states. + * + * In the first case, updateHook() is still called, and recover() allows + * you to go back into the Running state. In the second case, the + * errorHook() will be called instead of updateHook(). In Exception, the + * component is stopped and recover() needs to be called before starting + * it again. Finally, FatalError cannot be recovered. + */ + void updateHook(); + + /** This hook is called by Orocos when the component is in the + * RunTimeError state, at each activity step. See the discussion in + * updateHook() about triggering options. + * + * Call recover() to go back in the Runtime state. + */ + void errorHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Running to Stopped after stop() has been called. + */ + void stopHook(); + + /** This hook is called by Orocos when the state machine transitions + * from Stopped to PreOperational, requiring the call to configureHook() + * before calling start() again. + */ + void cleanupHook(); + }; +} + +#endif +