DUNE: Uniform Navigational Environment  2017.01.0
DUNE::Control::PathController Class Referenceabstract

Classes

struct  TrackingState
 

Public Member Functions

 PathController (std::string name, Tasks::Context &ctx)
 
virtual ~PathController (void)
 
void onEntityReservation (void)
 
void consume (const IMC::Brake *brake)
 
void consume (const IMC::EstimatedState *es)
 
void consume (const IMC::ControlLoops *cl)
 
void consume (const IMC::DesiredPath *dp)
 
void consume (const IMC::NavigationUncertainty *nu)
 
void consume (const IMC::Distance *dist)
 
void consume (const IMC::DesiredZ *zref)
 
void consume (const IMC::DesiredSpeed *dspeed)
 
virtual void onUpdateParameters (void)
 
virtual void onResourceInitialization (void)
 
virtual void onResourceRelease (void)
 
virtual void onPathActivation (void)
 
virtual void onPathDeactivation (void)
 
virtual void onPathStartup (const IMC::EstimatedState &state, const TrackingState &ts)
 
virtual void step (const IMC::EstimatedState &state, const TrackingState &ts)=0
 
virtual void loiter (const IMC::EstimatedState &state, const TrackingState &ts)
 
virtual bool hasSpecificZControl (void) const
 
virtual double getEta (const TrackingState &ts)
 
float getSpeed (void)
 
float getTimeFactor (void)
 
void signalError (const std::string &msg)
 
void enableControlLoops (uint32_t mask)
 
void disableControlLoops (uint32_t mask)
 
virtual bool sourceFilter (const IMC::EstimatedState *es)
 
void onMain (void)
 
- Public Member Functions inherited from DUNE::Tasks::Task
 Task (const std::string &name, Context &context)
 
virtual ~Task (void)
 
const char * getName (void) const
 
const char * getSystemName (void) const
 
unsigned int getSystemId (void) const
 
unsigned int getEntityId (void) const
 
unsigned int resolveEntity (const std::string &label) const
 
std::string resolveEntity (unsigned int id) const
 
DebugLevel getDebugLevel (void) const
 
uint16_t getActivationTime (void) const
 
uint16_t getDeactivationTime (void) const
 
unsigned int resolveSystemName (const std::string &name) const
 
const char * resolveSystemId (unsigned int id) const
 
void loadConfig (void)
 
void setPriority (unsigned int value)
 
unsigned int getPriority (void) const
 
void inf (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void war (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void err (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void cri (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void debug (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void trace (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void void spew (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void
void void 
dispatch (IMC::Message *msg, unsigned int flags=0)
 
void dispatch (IMC::Message &msg, unsigned int flags=0)
 
void dispatchReply (const IMC::Message &original, IMC::Message &msg, unsigned int flags=0)
 
void receive (const IMC::Message *msg)
 
void reserveEntities (void)
 
void resolveEntities (void)
 
void acquireResources (void)
 
void releaseResources (void)
 
void initializeResources (void)
 
void updateParameters (bool act_deact=true)
 
void writeParamsXML (std::ostream &os) const
 
const char * getEntityLabel (void) const
 
void setEntityLabel (const std::string &label)
 
- Public Member Functions inherited from DUNE::Tasks::AbstractTask
 AbstractTask (void)
 
virtual ~AbstractTask (void)
 
- Public Member Functions inherited from DUNE::Concurrency::Thread
 Thread (void)
 
virtual ~Thread (void)
 
int getProcessorUsage (void)
 
- Public Member Functions inherited from DUNE::Concurrency::Runnable
 Runnable (void)
 
virtual ~Runnable (void)
 
void start (void)
 
void stop (void)
 
void join (void)
 
void stopAndJoin (void)
 
void setPriority (Scheduler::Policy policy, unsigned priority)
 
unsigned getPriority (void)
 
State getState (void)
 
bool isCreated (void)
 
bool isStopping (void)
 
bool isRunning (void)
 
bool isStarting (void)
 
bool isDead (void)
 

Additional Inherited Members

- Public Types inherited from DUNE::Concurrency::Runnable
enum  State {
  StateStarting, StateRunning, StateStopping, StateDead,
  StateUnknown
}
 
- Protected Member Functions inherited from DUNE::Tasks::Task
void setEntityState (IMC::EntityState::StateEnum state, Status::Code code)
 
void setEntityState (IMC::EntityState::StateEnum state, const std::string &description)
 
IMC::EntityState::StateEnum getEntityState (void) const
 
unsigned int reserveEntity (const std::string &label)
 
template<typename E >
E * reserveEntity (const std::string &label)
 
Entities::BasicEntitygetLocalEntity (const std::string &label)
 
bool stopping (void)
 
bool isActive (void) const
 
bool isActivating (void) const
 
bool isDeactivating (void) const
 
void waitForMessages (double timeout)
 
void consumeMessages (void)
 
template<typename T >
Parameterparam (const std::string &name, T &var)
 
template<typename Y , typename T >
Parameterparam (const std::string &name, T &var)
 
template<typename T >
bool paramChanged (T &var)
 
void paramActive (Parameter::Scope def_scope, Parameter::Visibility def_visibility, bool def_value=false)
 
void setParamSectionEditor (const std::string &name)
 
template<typename M , typename T >
void bind (T *task_obj, void(T::*consumer)(const M *)=&T::consume)
 
template<typename T >
void bind (T *task_obj, const std::vector< uint32_t > &list)
 
template<typename T , typename M >
void bind (T *task_obj, const std::vector< uint32_t > &list, void(T::*consumer)(const M *)=&T::consume)
 
template<typename T >
void bind (T *task_obj, const std::vector< std::string > &list)
 
void requestActivation (void)
 
void requestDeactivation (void)
 
void activate (void)
 
void activationFailed (const std::string &reason)
 
void deactivate (void)
 
void deactivationFailed (const std::string &reason)
 
virtual bool onWriteParamsXML (std::ostream &os) const
 
virtual void onEntityResolution (void)
 
virtual void onReportEntityState (void)
 
virtual void onResourceAcquisition (void)
 
virtual void onRequestActivation (void)
 
virtual void onRequestDeactivation (void)
 
virtual void onQueryEntityParameters (const IMC::QueryEntityParameters *msg)
 
virtual void onSetEntityParameters (const IMC::SetEntityParameters *msg)
 
virtual void onPushEntityParameters (const IMC::PushEntityParameters *msg)
 
virtual void onPopEntityParameters (const IMC::PopEntityParameters *msg)
 
- Protected Member Functions inherited from DUNE::Concurrency::Thread
void startImpl (void)
 
void stopImpl (void)
 
void joinImpl (void)
 
void setPriorityImpl (Scheduler::Policy policy, unsigned priority)
 
unsigned getPriorityImpl (void)
 
- Protected Attributes inherited from DUNE::Tasks::Task
Contextm_ctx
 
std::vector
< Entities::BasicEntity * > 
m_entities
 

Constructor & Destructor Documentation

DUNE::Control::PathController::PathController ( std::string  name,
Tasks::Context ctx 
)
DUNE::Control::PathController::~PathController ( void  )
virtual

Destructor.

References DUNE::Memory::clear().

Member Function Documentation

void DUNE::Control::PathController::consume ( const IMC::Brake brake)

Consumer for Brake message.

Parameters
brakemessage to consume.

References DUNE::IMC::Brake::op, DUNE::IMC::Brake::OP_START, and DUNE::Time::Counter< T >::setTop().

void DUNE::Control::PathController::consume ( const IMC::EstimatedState es)

Consumer for EstimatedState message.

Parameters
esmessage to consume.

References DUNE::Control::PathController::TrackingState::LoiterData::center, DUNE::Control::PathController::TrackingState::course_error, DUNE::Tasks::Task::debug(), DUNE::Control::PathController::TrackingState::delta, DUNE::Tasks::Task::dispatch(), DUNE::Control::PathController::TrackingState::end, DUNE::IMC::PathControlState::end_lat, DUNE::IMC::PathControlState::end_lon, DUNE::Control::PathController::TrackingState::end_time, DUNE::IMC::EstimatedState::height, DUNE::Tasks::Task::inf(), DUNE::Tasks::Task::isActive(), DUNE::IMC::EstimatedState::lat, DUNE::Control::PathController::TrackingState::loiter, loiter(), DUNE::Control::PathController::TrackingState::loitering, DUNE::IMC::EstimatedState::lon, DUNE::Control::PathController::TrackingState::nearby, DUNE::Control::PathController::TrackingState::now, DUNE::Control::BottomTracker::onEstimatedState(), DUNE::IMC::Brake::op, DUNE::IMC::Brake::OP_STOP, DUNE::Time::Counter< T >::overflow(), DUNE::Control::PathController::TrackingState::LoiterData::radius, DUNE::Time::Counter< T >::setTop(), signalError(), sourceFilter(), DUNE::Control::PathController::TrackingState::start, DUNE::IMC::PathControlState::start_lat, DUNE::IMC::PathControlState::start_lon, step(), DUNE::Control::PathController::TrackingState::track_pos, DUNE::Math::trimValue(), DUNE::Control::PathController::TrackingState::Coord::x, DUNE::Control::PathController::TrackingState::TrackCoord::x, and DUNE::Control::PathController::TrackingState::Coord::y.

void DUNE::Control::PathController::consume ( const IMC::DesiredPath dp)

Consumer for DesiredPath message.

Parameters
dpmessage to consume.

References DUNE::Control::PathController::TrackingState::LoiterData::center, DUNE::IMC::CL_ALTITUDE, DUNE::IMC::CL_DEPTH, DUNE::IMC::CL_SPEED, DUNE::Control::PathController::TrackingState::LoiterData::clockwise, DUNE::Control::PathController::TrackingState::course_error, DUNE::Math::Angles::degrees(), DUNE::Control::PathController::TrackingState::delta, DUNE::Tasks::DF_LOOP_BACK, disableControlLoops(), DUNE::Tasks::Task::dispatch(), enableControlLoops(), DUNE::Control::PathController::TrackingState::end, DUNE::IMC::DesiredPath::end_lat, DUNE::IMC::PathControlState::end_lat, DUNE::IMC::DesiredPath::end_lon, DUNE::IMC::PathControlState::end_lon, DUNE::Control::PathController::TrackingState::end_time, DUNE::IMC::DesiredPath::end_z, DUNE::IMC::PathControlState::end_z, DUNE::IMC::DesiredPath::end_z_units, DUNE::IMC::PathControlState::end_z_units, DUNE::IMC::DesiredPath::FL_CCLOCKW, DUNE::IMC::DesiredPath::FL_DIRECT, DUNE::IMC::DesiredPath::FL_LOITER_CURR, DUNE::IMC::DesiredPath::FL_NO_Z, DUNE::IMC::PathControlState::FL_NO_Z, DUNE::IMC::DesiredPath::FL_START, DUNE::IMC::DesiredPath::flags, DUNE::IMC::PathControlState::flags, DUNE::Coordinates::getBearingAndRange(), hasSpecificZControl(), DUNE::IMC::EstimatedState::height, DUNE::Tasks::Task::inf(), DUNE::Tasks::Task::isActive(), DUNE::IMC::EstimatedState::lat, DUNE::Control::PathController::TrackingState::loiter, DUNE::Control::PathController::TrackingState::loitering, DUNE::IMC::EstimatedState::lon, DUNE::IMC::DesiredPath::lradius, DUNE::Control::PathController::TrackingState::nearby, DUNE::Math::Angles::normalizeRadian(), DUNE::Control::PathController::TrackingState::now, DUNE::Control::BottomTracker::onDesiredZ(), onPathStartup(), DUNE::IMC::DesiredPath::path_ref, DUNE::IMC::PathControlState::path_ref, DUNE::IMC::EstimatedState::psi, DUNE::Control::PathController::TrackingState::LoiterData::radius, DUNE::Coordinates::setBearingAndRange(), signalError(), DUNE::IMC::DesiredPath::speed, DUNE::IMC::DesiredSpeed::speed_units, DUNE::IMC::DesiredPath::speed_units, DUNE::Control::PathController::TrackingState::start, DUNE::IMC::DesiredPath::start_lat, DUNE::IMC::PathControlState::start_lat, DUNE::IMC::DesiredPath::start_lon, DUNE::IMC::PathControlState::start_lon, DUNE::Control::PathController::TrackingState::start_time, DUNE::IMC::DesiredPath::start_z, DUNE::IMC::PathControlState::start_z, DUNE::IMC::DesiredPath::start_z_units, DUNE::IMC::PathControlState::start_z_units, DUNE::Coordinates::toWGS84(), DUNE::Tasks::Task::trace(), DUNE::Control::PathController::TrackingState::track_bearing, DUNE::Control::PathController::TrackingState::track_length, DUNE::Control::PathController::TrackingState::track_pos, DUNE::IMC::DesiredZ::value, DUNE::IMC::DesiredSpeed::value, DUNE::Tasks::Task::war(), DUNE::Control::PathController::TrackingState::Coord::x, DUNE::Control::PathController::TrackingState::TrackCoord::x, DUNE::IMC::EstimatedState::x, DUNE::Control::PathController::TrackingState::Coord::y, DUNE::Control::PathController::TrackingState::TrackCoord::y, DUNE::IMC::EstimatedState::y, DUNE::Control::PathController::TrackingState::Coord::z, DUNE::Control::PathController::TrackingState::TrackCoord::z, DUNE::IMC::EstimatedState::z, DUNE::IMC::Z_ALTITUDE, DUNE::Control::PathController::TrackingState::z_control, DUNE::IMC::Z_DEPTH, DUNE::IMC::Z_HEIGHT, and DUNE::IMC::DesiredZ::z_units.

void DUNE::Control::PathController::consume ( const IMC::NavigationUncertainty nu)

Consumer for NavigationUncertainty message.

Parameters
numessage to consume.

References DUNE::IMC::NavigationUncertainty::x, and DUNE::IMC::NavigationUncertainty::y.

void DUNE::Control::PathController::consume ( const IMC::Distance dist)

Consumer for Distance message.

Parameters
distmessage to consume.

References DUNE::Control::BottomTracker::onDistance().

void DUNE::Control::PathController::consume ( const IMC::DesiredZ zref)

Consumer for DesiredZ message.

Parameters
zrefmessage to consume.

References DUNE::Control::BottomTracker::onDesiredZ().

void DUNE::Control::PathController::consume ( const IMC::DesiredSpeed dspeed)

Consumer for DesiredSpeed message.

Parameters
dspeedmessage to consume.

References DUNE::Control::BottomTracker::onDesiredSpeed().

void DUNE::Control::PathController::disableControlLoops ( uint32_t  mask)
inline

Disable control loops (need to use only if control mode changes during path control, not on deactivation).

Parameters
maskcontrol loop mask

References DUNE::IMC::ControlLoops::CL_DISABLE.

Referenced by consume(), and Control::Path::PurePursuit::Task::onPathDeactivation().

double DUNE::Control::PathController::getEta ( const TrackingState ts)
virtual

Default implementation for calculating estimated time of arrival, that can be overridden to change how maneuver completion is signalized.

Parameters
tsthe current tracking state
time_factorfor time of arrival factor
speed
Returns
the calculated estimated time of arrival

References getSpeed(), getTimeFactor(), DUNE::Math::norm(), DUNE::Control::PathController::TrackingState::track_length, DUNE::Control::PathController::TrackingState::track_pos, DUNE::Control::PathController::TrackingState::TrackCoord::x, and DUNE::Control::PathController::TrackingState::TrackCoord::y.

float DUNE::Control::PathController::getSpeed ( void  )
inline
float DUNE::Control::PathController::getTimeFactor ( void  )
inline

Referenced by getEta().

virtual bool DUNE::Control::PathController::hasSpecificZControl ( void  ) const
inlinevirtual

Flagging method indicating if controller wishes to handle altitude/depth control.

If not (the default) depth or altitude references will be fired at the start of a path.

Returns
false (at the base class level)

Referenced by consume().

void DUNE::Control::PathController::onEntityReservation ( void  )
virtual

Entity reservation callback.

Reimplemented from DUNE::Tasks::Task.

References DUNE::Tasks::Task::getEntityLabel(), and DUNE::Utils::String::str().

void DUNE::Control::PathController::onMain ( void  )
virtual
virtual void DUNE::Control::PathController::onPathActivation ( void  )
inlinevirtual

Handler for path control activation.

This is called when path control is activated. By default it does nothing.

Reimplemented in Control::Path::ILOS::Task, Control::Path::LOSnSMC::Task, Control::Path::VectorField::Task, Control::UAV::LOS::Task, and Control::Path::PurePursuit::Task.

virtual void DUNE::Control::PathController::onPathDeactivation ( void  )
inlinevirtual

Handler for path control deactivation.

This is called when path control is deactivated. By default it does nothing.

Reimplemented in Control::Path::PurePursuit::Task.

virtual void DUNE::Control::PathController::onPathStartup ( const IMC::EstimatedState state,
const TrackingState ts 
)
inlinevirtual

Handler for the startup of a new path.

The default handler does nothing and can be overriden. This is called when a new path is started (several paths may be executed between activation and deactivation).

Parameters
statecurrent navigational state.
tsinitial tracking state.

Referenced by consume().

void DUNE::Control::PathController::onResourceInitialization ( void  )
virtual

On resource initialization This can be overriden but in that case this parent class implementation MUST be called.

Reimplemented from DUNE::Tasks::Task.

References DUNE::Tasks::Task::requestDeactivation().

void DUNE::Control::PathController::onResourceRelease ( void  )
virtual

On resource aquisition This can be overriden but in that case this parent class implementation MUST be called.

Reimplemented from DUNE::Tasks::Task.

void DUNE::Control::PathController::signalError ( const std::string &  msg)

Signal an error.

This method should be used by subclasses to signal an error condition.

Parameters
msgerror message

References DUNE::Tasks::Task::err().

Referenced by consume().

virtual bool DUNE::Control::PathController::sourceFilter ( const IMC::EstimatedState es)
inlinevirtual

Handler for EstimatedState source id filter.

This is called when an EstimatedState is received. By default it only passes EstimatedState messages from the system itself.

Parameters
[in]esEstimatedState message.
Returns
true if the message is NOT allowed to pass.

Reimplemented in DUNE::Control::ProxyPathController.

References DUNE::IMC::Message::getSource(), and DUNE::Tasks::Task::getSystemId().

Referenced by consume().

virtual void DUNE::Control::PathController::step ( const IMC::EstimatedState state,
const TrackingState ts 
)
pure virtual

Abstract method for controller step that must be provided by subclasses.

Parameters
statenavigation state
tstracking state information

Referenced by consume(), and loiter().

Collaboration diagram for DUNE::Control::PathController:
Collaboration graph