DUNE::Control::PathController Class Reference

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::BasicEntity *	getLocalEntity (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 >
Parameter &	param (const std::string &name, T &var)
template<typename Y , typename T >
Parameter &	param (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
Context &	m_ctx
std::vector < Entities::BasicEntity * >	m_entities

Constructor & Destructor Documentation

DUNE::Control::PathController::PathController	(	std::string	name,
		Tasks::Context &	ctx
	)

Constructor.

References DUNE::Tasks::Context::config, DUNE::Tasks::Parameter::defaultValue(), DUNE::Parsers::Config::get(), DUNE::Tasks::Task::m_ctx, and DUNE::Tasks::Task::param().

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

brake

message 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

es	message 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::ControlLoops *

cl

)

Consumer for ControlLoops message.

Parameters

cl	message to consume.

References DUNE::IMC::ControlLoops::CL_ENABLE, DUNE::IMC::CL_PATH, DUNE::IMC::ControlLoops::enable, DUNE::Tasks::Task::isActive(), DUNE::IMC::ControlLoops::mask, DUNE::Tasks::Task::requestActivation(), DUNE::Tasks::Task::requestDeactivation(), and DUNE::IMC::ControlLoops::scope_ref.

void DUNE::Control::PathController::consume

(

const IMC::DesiredPath *

dp

)

Consumer for DesiredPath message.

Parameters

dp	message 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

nu	message 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

dist	message to consume.

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

void DUNE::Control::PathController::consume

(

const IMC::DesiredZ *

zref

)

Consumer for DesiredZ message.

Parameters

zref	message to consume.

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

void DUNE::Control::PathController::consume

(

const IMC::DesiredSpeed *

dspeed

)

Consumer for DesiredSpeed message.

Parameters

dspeed

message 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

mask	control loop mask

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

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

void DUNE::Control::PathController::enableControlLoops ( uint32_t  mask )

inline

Enable control loops.

Parameters

mask	control loop mask

References DUNE::IMC::ControlLoops::CL_ENABLE.

Referenced by consume(), Control::Path::PurePursuit::Task::onPathActivation(), Control::UAV::LOS::Task::onPathActivation(), Control::Path::VectorField::Task::onPathActivation(), Control::Path::LOSnSMC::Task::onPathActivation(), and Control::Path::ILOS::Task::onPathActivation().

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

ts	the current tracking state
time_factor	for 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

References DUNE::Control::PathController::TrackingState::speed.

Referenced by getEta().

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::loiter ( const IMC::EstimatedState &  state, const TrackingState &  ts  )

virtual

Default implementation for loiter control, that can be overriden for a controller specific implementation.

Parameters

state	navigation state
ts	tracking state information

References DUNE::Control::PathController::TrackingState::LoiterData::clockwise, DUNE::Control::PathController::TrackingState::end, DUNE::Coordinates::getBearing(), DUNE::Control::PathController::TrackingState::loiter, DUNE::Control::PathController::TrackingState::los_angle, DUNE::Control::PathController::TrackingState::LoiterData::radius, DUNE::Coordinates::setBearingAndRange(), DUNE::Control::PathController::TrackingState::start, step(), DUNE::Control::PathController::TrackingState::track_bearing, DUNE::Control::PathController::TrackingState::track_length, DUNE::Control::PathController::TrackingState::track_pos, and DUNE::Control::PathController::TrackingState::TrackCoord::x.

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

Task method.

Implements DUNE::Tasks::Task.

References DUNE::Tasks::Task::stopping(), and DUNE::Tasks::Task::waitForMessages().

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

state	current navigational state.
ts	initial 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::onUpdateParameters ( void  )

virtual

Handler for parameter updates.

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

Reimplemented from DUNE::Tasks::Task.

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

References DUNE::Control::BottomTracker, DUNE::Control::PathController::TrackingState::cc, DUNE::Memory::clear(), DUNE::Control::PathController::TrackingState::end_time, DUNE::Control::PathController::TrackingState::loitering, DUNE::Control::PathController::TrackingState::nearby, DUNE::Tasks::Task::paramChanged(), DUNE::Math::Angles::radians(), and DUNE::Control::PathController::TrackingState::z_control.

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

msg	error 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]

es

EstimatedState 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

state	navigation state
ts	tracking state information

Referenced by consume(), and loiter().

Collaboration diagram for DUNE::Control::PathController: