DUNE: Uniform Navigational Environment
2.4.2
|
Public Member Functions | |
Task (const std::string &name, Tasks::Context &ctx) | |
void | onUpdateParameters (void) |
void | onEntityResolution (void) |
void | onResourceAcquisition (void) |
void | onResourceRelease (void) |
void | consume (const IMC::LeaderState *msg) |
void | consume (const IMC::IndicatedSpeed *msg) |
void | consume (const IMC::EstimatedStreamVelocity *msg) |
void | consume (const IMC::SimulatedState *msg) |
void | consume (const IMC::Acceleration *msg) |
void | consume (const IMC::DesiredRoll *msg) |
void | consume (const IMC::DesiredSpeed *msg) |
void | consume (const IMC::EstimatedState *msg) |
void | task (void) |
void | setLeaderState (const IMC::LeaderState *msg) |
Matrix | matrixRotRbody2gnd (float roll, float pitch) |
void | checkParameters (void) |
void | teamPeriodicUpdate (const double &d_time) |
void | teamUnevenUpdate (const double &d_time) |
void | formationControl (const Matrix &md_uav_state, const Matrix &md_uav_accel, const int &ind_uav, const double &d_time_step, Matrix *vd_cmd) |
Public Member Functions inherited from DUNE::Tasks::Periodic | |
Periodic (const std::string &name, Context &ctx) | |
virtual | ~Periodic (void) |
void | setFrequency (double freq) |
double | getFrequency (void) const |
double | getRunTime (void) const |
unsigned | getRunCount (void) const |
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 |
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 |
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) |
State | getState (void) |
bool | isCreated (void) |
bool | isStopping (void) |
bool | isRunning (void) |
bool | isStarting (void) |
bool | isDead (void) |
Public Attributes | |
Arguments | m_args |
std::vector< UAVSimulation * > | m_models |
UAVSimulation * | m_model |
Matrix | m_position |
Matrix | m_velocity |
IMC::SimulatedState | m_sstate |
double | m_last_leader_update |
double | m_last_ctrl_update |
Matrix | m_last_state_estim |
Matrix | m_last_simctrl_update |
double | m_last_time_debug |
double | m_last_time_trace |
Matrix | m_uav_state |
Matrix | m_uav_accel |
double | m_airspeed |
Matrix | m_wind |
Matrix | m_uav_ctrl |
IMC::DesiredRoll | m_bank_cmd |
IMC::DesiredSpeed | m_airspeed_cmd |
int | m_uav_n |
bool | m_team_state_init |
bool | m_valid_airspeed |
double | m_frequency |
IMC::LeaderState | m_init_leader |
bool | m_init_leader_set |
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 | |
const char * | getEntityLabel (void) const |
void | setEntityLabel (const std::string &label) |
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) |
unsigned int | resolveEntity (const std::string &label) const |
std::string | resolveEntity (unsigned int id) const |
bool | stopping (void) |
bool | isActive (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 > | |
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 void | onEntityReservation (void) |
virtual void | onReportEntityState (void) |
virtual void | onResourceInitialization (void) |
virtual void | onRequestActivation (void) |
virtual void | onRequestDeactivation (void) |
virtual void | onActivation (void) |
virtual void | onDeactivation (void) |
Protected Member Functions inherited from DUNE::Concurrency::Thread | |
void | startImpl (void) |
void | stopImpl (void) |
void | joinImpl (void) |
void | setPriorityImpl (Scheduler::Policy policy, unsigned priority) |
Protected Attributes inherited from DUNE::Tasks::Task | |
Context & | m_ctx |
|
inline |
References Maneuver::VehicleFormation::FormCollAvoid::Arguments::acc_safety_marg, Maneuver::VehicleFormation::FormCollAvoid::Arguments::accel_lim_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::bank_lim, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_bank, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, DUNE::Tasks::Parameter::defaultValue(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_frame, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, Maneuver::VehicleFormation::FormCollAvoid::Arguments::gaccel, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lat, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lon, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_roll, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_yaw, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_boundary, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_deconfliction, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_lateral, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_leader, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_longitudinal, m_args, DUNE::Math::min(), DUNE::Tasks::Task::param(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::plan, DUNE::Math::Angles::radians(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, and Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_n.
|
inline |
References DUNE::Math::Matrix::columns(), DUNE::Tasks::Task::err(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, m_args, m_uav_n, DUNE::Math::Matrix::resizeAndKeep(), DUNE::Math::Matrix::rows(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_n, and DUNE::Tasks::Task::war().
Referenced by onResourceAcquisition(), and onUpdateParameters().
|
inline |
|
inline |
|
inline |
References m_wind.
|
inline |
References DUNE::Simulation::UAVSimulation::command(), DUNE::Math::Matrix::get(), DUNE::Tasks::Task::isActive(), m_args, m_last_state_estim, m_model, m_uav_n, m_uav_state, m_wind, DUNE::Math::Matrix::norm_2(), DUNE::Tasks::Task::requestActivation(), DUNE::Tasks::Task::resolveSystemName(), DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::setPosition(), DUNE::Simulation::UAVSimulation::setVelocity(), DUNE::Math::Matrix::toDCM(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, and DUNE::Math::Matrix::vertCat().
|
inline |
|
inline |
|
inline |
|
inline |
References DUNE::Simulation::UAVSimulation::command(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, DUNE::Tasks::Task::debug(), DUNE::Tasks::Task::dispatch(), formationControl(), DUNE::Math::Matrix::get(), DUNE::Tasks::Task::getSystemId(), DUNE::Tasks::Task::isActive(), m_airspeed_cmd, m_args, m_bank_cmd, m_last_ctrl_update, m_last_simctrl_update, m_last_state_estim, m_models, m_team_state_init, m_uav_accel, m_uav_ctrl, m_uav_n, m_uav_state, m_valid_airspeed, DUNE::Tasks::Task::requestActivation(), DUNE::Tasks::Task::resolveSystemId(), DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::setPosition(), DUNE::Simulation::UAVSimulation::setVelocity(), teamPeriodicUpdate(), teamUnevenUpdate(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, and DUNE::Math::Matrix::vertCat().
|
inline |
References Maneuver::VehicleFormation::FormCollAvoid::Arguments::acc_safety_marg, Maneuver::VehicleFormation::FormCollAvoid::Arguments::accel_lim_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::bank_lim, DUNE::Math::Matrix::column(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_frame, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, Maneuver::VehicleFormation::FormCollAvoid::Arguments::gaccel, DUNE::Math::Matrix::get(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_boundary, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_deconfliction, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_lateral, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_leader, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_longitudinal, m_args, m_uav_n, m_wind, DUNE::Math::Matrix::norm_2(), DUNE::Math::Matrix::row(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, DUNE::Math::Matrix::set(), DUNE::Tasks::Task::spew(), DUNE::Math::sum(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_min, DUNE::Math::transpose(), DUNE::Math::trimValue(), and Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind.
Referenced by consume(), and teamPeriodicUpdate().
|
inline |
|
inlinevirtual |
Called when the task is instructed to resolve all the entity identifiers it needs for normal execution.
See resolveEntity(). Derived classes that need to resolve entity identifiers should override this function.
Reimplemented from DUNE::Tasks::Task.
|
inlinevirtual |
Called when the task is instructed to acquire resources whose configuration is defined by run-time parameters.
Reimplemented from DUNE::Tasks::Task.
References Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_bank, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_speed, checkParameters(), DUNE::Simulation::UAVSimulation::command(), DUNE::Tasks::Task::debug(), DUNE::Math::Angles::degrees(), DUNE::Tasks::Task::dispatch(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, DUNE::Math::Matrix::get(), DUNE::Tasks::Periodic::getFrequency(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lat, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lon, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_roll, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_yaw, m_airspeed, m_airspeed_cmd, m_args, m_frequency, m_init_leader, m_last_ctrl_update, m_last_leader_update, m_last_simctrl_update, m_last_state_estim, m_model, m_models, m_position, DUNE::Simulation::UAVSimulation::m_sim_type, m_uav_ctrl, m_uav_n, m_uav_state, m_velocity, m_wind, DUNE::Math::Matrix, Maneuver::VehicleFormation::FormCollAvoid::Arguments::plan, DUNE::Math::Angles::radians(), setLeaderState(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, DUNE::Tasks::Task::spew(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, DUNE::Simulation::UAVSimulation, and DUNE::Math::Matrix::vertCat().
|
inlinevirtual |
Called when the task is instructed to release resources.
Derived classes that override this function must not assume that any resource was previously acquired. This function must be implemented in such a way that it can be called at any time.
Reimplemented from DUNE::Tasks::Task.
|
inlinevirtual |
Called when the task is instructed to update its run-time parameters.
Derived classes that need to compute auxiliary values based on run-time parameters should override this function.
Reimplemented from DUNE::Tasks::Task.
References checkParameters().
|
inline |
References DUNE::Tasks::Task::dispatch(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Tasks::Task::getSystemId(), DUNE::Tasks::Task::isActive(), m_last_leader_update, m_model, m_position, m_sstate, m_velocity, m_wind, DUNE::Simulation::UAVSimulation::m_wind, DUNE::Tasks::Task::requestActivation(), DUNE::Simulation::UAVSimulation::setPosition(), and DUNE::Simulation::UAVSimulation::setVelocity().
Referenced by consume(), and onResourceAcquisition().
|
inlinevirtual |
The task to be executed on each cycle.
Implements DUNE::Tasks::Periodic.
References DUNE::Tasks::Task::consumeMessages(), DUNE::Tasks::Task::dispatch(), DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Tasks::Task::getSystemId(), DUNE::Tasks::Task::getSystemName(), DUNE::Simulation::UAVSimulation::getVelocity(), DUNE::Tasks::Task::isActive(), m_args, m_init_leader, m_last_leader_update, m_model, m_position, m_sstate, m_velocity, DUNE::Simulation::UAVSimulation::m_wind, DUNE::Tasks::Task::spew(), teamPeriodicUpdate(), DUNE::Math::Matrix::toDCM(), DUNE::Math::transpose(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, and DUNE::Simulation::UAVSimulation::update().
|
inline |
Update team simulated state for standard time periods.
References DUNE::Simulation::UAVSimulation::command(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, formationControl(), DUNE::Math::Matrix::get(), m_args, m_frequency, m_last_simctrl_update, m_last_state_estim, m_model, m_models, m_uav_ctrl, m_uav_n, m_uav_state, DUNE::Math::Matrix::set(), teamUnevenUpdate(), and DUNE::Simulation::UAVSimulation::update().
|
inline |
Update team simulated state for uneven time periods.
References DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Simulation::UAVSimulation::getVelocity(), m_last_state_estim, m_model, m_models, m_uav_n, m_uav_state, DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::update(), and DUNE::Math::Matrix::vertCat().
Referenced by consume(), and teamPeriodicUpdate().
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed |
Referenced by consume(), and onResourceAcquisition().
IMC::DesiredSpeed Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed_cmd |
Referenced by consume(), and onResourceAcquisition().
Arguments Maneuver::VehicleFormation::FormCollAvoid::Task::m_args |
Task arguments.
Referenced by checkParameters(), consume(), formationControl(), onResourceAcquisition(), Task(), task(), and teamPeriodicUpdate().
IMC::DesiredRoll Maneuver::VehicleFormation::FormCollAvoid::Task::m_bank_cmd |
Vehicle commands.
Referenced by consume().
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_frequency |
Simulation process frequency.
Referenced by onResourceAcquisition(), and teamPeriodicUpdate().
IMC::LeaderState Maneuver::VehicleFormation::FormCollAvoid::Task::m_init_leader |
Leader initial state.
Referenced by onResourceAcquisition(), and task().
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_init_leader_set |
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_ctrl_update |
Referenced by consume(), and onResourceAcquisition().
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_leader_update |
Simulation and control time variables.
Referenced by onResourceAcquisition(), setLeaderState(), and task().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_simctrl_update |
Referenced by consume(), onResourceAcquisition(), and teamPeriodicUpdate().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_estim |
Referenced by consume(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_debug |
Last time debug information was shown.
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_trace |
UAVSimulation* Maneuver::VehicleFormation::FormCollAvoid::Task::m_model |
Leader vehicle model.
Referenced by consume(), onResourceAcquisition(), setLeaderState(), task(), teamPeriodicUpdate(), and teamUnevenUpdate().
std::vector<UAVSimulation*> Maneuver::VehicleFormation::FormCollAvoid::Task::m_models |
Simulated vehicles' models.
Referenced by consume(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_position |
Leader's simulated position (X,Y,Z,phi,theta,psi).
Referenced by onResourceAcquisition(), setLeaderState(), and task().
IMC::SimulatedState Maneuver::VehicleFormation::FormCollAvoid::Task::m_sstate |
Leader's simulated state.
Referenced by setLeaderState(), and task().
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_state_init |
Process logic control variables.
Referenced by consume().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ctrl |
System command variables.
Referenced by consume(), onResourceAcquisition(), and teamPeriodicUpdate().
int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_n |
Number of team vehicles.
Referenced by checkParameters(), consume(), formationControl(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_state |
System state variables.
Referenced by consume(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_valid_airspeed |
Referenced by consume().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_velocity |
Leader's simulated velocity (u,v,w,p,q,r).
Referenced by onResourceAcquisition(), setLeaderState(), and task().
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind |
Referenced by consume(), formationControl(), onResourceAcquisition(), and setLeaderState().