float
Duration::parse(const IMC::Elevator* maneuver, Position& last_pos, float last_dur,
std::vector<float>& durations, const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
float goto_dist = distance3D(pos, last_pos);
float amplitude = std::fabs(last_pos.z - maneuver->end_z);
float real_dist = amplitude / std::sin(c_rated_pitch);
float travelled = goto_dist + real_dist;
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
durations.push_back(travelled / speed + last_dur);
return durations.back();
}
bool
TimeProfile::parse(const IMC::Elevator* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Position pos;
extractPosition(maneuver, pos);
float goto_dist = distance3D(pos, last_pos);
float amplitude = std::fabs(last_pos.z - maneuver->end_z);
float real_dist = amplitude / std::sin(c_rated_pitch);
float travelled = goto_dist + real_dist;
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
float duration = travelled / speed;
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, duration));
m_accum_dur->addDuration(duration);
return true;
}
float
Duration::parse(const IMC::YoYo* maneuver, Position& last_pos, float last_dur,
std::vector<float>& durations, const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
durations.push_back(travelled / speed + last_dur);
return durations.back();
}
bool
TimeProfile::parse(const IMC::YoYo* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
float duration = travelled / speed;
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, duration));
m_accum_dur->addDuration(duration);
return true;
}
bool
TimeProfile::parse(const IMC::PopUp* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
// Travel time
float travel_time;
if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0)
{
Position pos;
extractPosition(maneuver, pos);
float travelled = distance3D(pos, last_pos);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
travel_time = travelled / speed;
last_pos = pos;
}
else
{
travel_time = 0;
}
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, travel_time));
// Rising time and descending time
float rising_time;
float descending_time;
if (maneuver->z_units == IMC::Z_DEPTH)
{
rising_time = (std::fabs(last_pos.z) / std::sin(c_rated_pitch)) / speed;
descending_time = (std::fabs(maneuver->z) / std::sin(c_rated_pitch)) / speed;
}
else // altitude, assume zero
{
rising_time = 0.0;
descending_time = 0.0;
}
// surface time
float surface_time = c_fix_time;
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, rising_time));
m_speed_vec->push_back(SpeedProfile(0.0, 0, surface_time));
m_speed_vec->push_back(SpeedProfile(maneuver, descending_time));
m_accum_dur->addDuration(travel_time + rising_time + surface_time + descending_time);
return true;
}
float
Duration::parse(const IMC::PopUp* maneuver, Position& last_pos, float last_dur,
std::vector<float>& durations, const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
// Travel time
float travel_time;
if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0)
{
Position pos;
extractPosition(maneuver, pos);
float travelled = distance3D(pos, last_pos);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
travel_time = travelled / speed;
last_pos = pos;
}
else
{
travel_time = 0;
}
// Rising time
float rising_time;
if (maneuver->z_units == IMC::Z_DEPTH)
rising_time = std::fabs(last_pos.z) / speed;
else // altitude, assume zero
rising_time = 0.0;
// surface time
bool wait = (maneuver->flags & IMC::PopUp::FLG_WAIT_AT_SURFACE) != 0;
float surface_time = wait ? maneuver->duration : c_fix_time;
durations.push_back(travel_time + rising_time + surface_time + last_dur);
return durations.back();
}
float
Duration::parseSimple(const Type* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
float travelled = distance3D(pos, last_pos);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
return travelled / speed;
}
float
Duration::parseSimple(const Type* maneuver, Position& last_pos,
float last_dur, std::vector<float>& durations,
const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
float travelled = distance3D(pos, last_pos);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
durations.push_back(travelled / speed + last_dur);
return durations[0];
}
bool
Duration::parse(const IMC::YoYo* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
m_accum_dur->addDuration(travelled / speed);
return true;
}
float
TimeProfile::parseSimple(const Type* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
float travelled = distance3D(pos, last_pos);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
float duration = travelled / speed;
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, duration));
return duration;
}
Duration::ManeuverDuration::const_iterator
Duration::parse(const std::vector<IMC::PlanManeuver*>& nodes,
const IMC::EstimatedState* state,
ManeuverDuration& man_durations,
const SpeedConversion& speed_conv)
{
Position pos;
extractPosition(state, pos);
float last_duration = 0.0;
std::vector<IMC::PlanManeuver*>::const_iterator itr = nodes.begin();
for (; itr != nodes.end(); ++itr)
{
if ((*itr)->data.isNull())
return man_durations.end();
IMC::Message* msg = (*itr)->data.get();
std::vector<float> durations;
switch (msg->getId())
{
#ifdef DUNE_IMC_GOTO
case DUNE_IMC_GOTO:
last_duration = parse(dynamic_cast<IMC::Goto*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_STATIONKEEPING
case DUNE_IMC_STATIONKEEPING:
last_duration = parse(dynamic_cast<IMC::StationKeeping*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_LOITER
case DUNE_IMC_LOITER:
last_duration = parse(dynamic_cast<IMC::Loiter*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_FOLLOWPATH
case DUNE_IMC_FOLLOWPATH:
last_duration = parse(dynamic_cast<IMC::FollowPath*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_ROWS
case DUNE_IMC_ROWS:
last_duration = parse(dynamic_cast<IMC::Rows*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_YOYO
case DUNE_IMC_YOYO:
last_duration = parse(dynamic_cast<IMC::YoYo*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_ELEVATOR
case DUNE_IMC_ELEVATOR:
last_duration = parse(dynamic_cast<IMC::Elevator*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
#ifdef DUNE_IMC_POPUP
case DUNE_IMC_POPUP:
last_duration = parse(dynamic_cast<IMC::PopUp*>(msg), pos,
last_duration, durations, speed_conv);
break;
#endif
default:
last_duration = -1.0;
break;
}
if (last_duration < 0.0)
{
if (man_durations.empty() || itr == nodes.begin())
return man_durations.end();
// return the duration from the previously computed maneuver
ManeuverDuration::const_iterator dtr;
dtr = man_durations.find((*(--itr))->maneuver_id);
if (dtr->second.empty())
return man_durations.end();
return dtr;
}
std::pair<std::string, std::vector<float> > ent((*itr)->maneuver_id, durations);
man_durations.insert(ent);
}
return man_durations.find(nodes.back()->maneuver_id);
}
Duration::ManeuverDuration::const_iterator
Duration::parse(const std::vector<IMC::PlanManeuver*>& nodes,
const IMC::EstimatedState* state)
{
Position pos;
extractPosition(state, pos);
std::vector<IMC::PlanManeuver*>::const_iterator itr = nodes.begin();
for (; itr != nodes.end(); ++itr)
{
if ((*itr)->data.isNull())
return m_durations.end();
IMC::Message* msg = (*itr)->data.get();
float last_duration = -1.0;
if (m_accum_dur != NULL)
{
if (m_accum_dur->size())
last_duration = m_accum_dur->getLastDuration();
}
Memory::clear(m_accum_dur);
m_accum_dur = new AccumulatedDurations(last_duration);
bool parsed = false;
switch (msg->getId())
{
case DUNE_IMC_GOTO:
parsed = parse(static_cast<IMC::Goto*>(msg), pos);
break;
case DUNE_IMC_STATIONKEEPING:
parsed = parse(static_cast<IMC::StationKeeping*>(msg), pos);
break;
case DUNE_IMC_LOITER:
parsed = parse(static_cast<IMC::Loiter*>(msg), pos);
break;
case DUNE_IMC_FOLLOWPATH:
parsed = parse(static_cast<IMC::FollowPath*>(msg), pos);
break;
case DUNE_IMC_ROWS:
parsed = parse(static_cast<IMC::Rows*>(msg), pos);
break;
case DUNE_IMC_YOYO:
parsed = parse(static_cast<IMC::YoYo*>(msg), pos);
break;
case DUNE_IMC_ELEVATOR:
parsed = parse(static_cast<IMC::Elevator*>(msg), pos);
break;
case DUNE_IMC_POPUP:
parsed = parse(static_cast<IMC::PopUp*>(msg), pos);
break;
case DUNE_IMC_COMPASSCALIBRATION:
parsed = parse(static_cast<IMC::CompassCalibration*>(msg), pos);
break;
default:
parsed = false;
break;
}
if (!parsed)
{
if (m_durations.empty() || itr == nodes.begin())
return m_durations.end();
// return the duration from the previously computed maneuver
ManeuverDuration::const_iterator dtr;
dtr = m_durations.find((*(--itr))->maneuver_id);
if (dtr->second.empty())
return m_durations.end();
return dtr;
}
std::pair<std::string, std::vector<float> > ent((*itr)->maneuver_id,
m_accum_dur->vec);
m_durations.insert(ent);
}
Memory::clear(m_accum_dur);
return m_durations.find(nodes.back()->maneuver_id);
}
void
TimeProfile::parse(const std::vector<IMC::PlanManeuver*>& nodes,
const IMC::EstimatedState* state)
{
if (!m_valid_model)
{
m_last_valid.clear();
return;
}
Position pos;
extractPosition(state, pos);
std::vector<IMC::PlanManeuver*>::const_iterator itr = nodes.begin();
for (; itr != nodes.end(); ++itr)
{
if ((*itr)->data.isNull())
return;
IMC::Message* msg = (*itr)->data.get();
float last_duration = -1.0;
if (m_accum_dur != NULL)
{
if (m_accum_dur->size())
last_duration = m_accum_dur->getLastDuration();
}
Memory::clear(m_accum_dur);
m_accum_dur = new TimeProfile::AccumulatedDurations(last_duration);
Memory::clear(m_speed_vec);
m_speed_vec = new std::vector<SpeedProfile>();
bool parsed = false;
switch (msg->getId())
{
case DUNE_IMC_GOTO:
parsed = parse(static_cast<IMC::Goto*>(msg), pos);
break;
case DUNE_IMC_STATIONKEEPING:
parsed = parse(static_cast<IMC::StationKeeping*>(msg), pos);
break;
case DUNE_IMC_LOITER:
parsed = parse(static_cast<IMC::Loiter*>(msg), pos);
break;
case DUNE_IMC_FOLLOWPATH:
parsed = parse(static_cast<IMC::FollowPath*>(msg), pos);
break;
case DUNE_IMC_ROWS:
parsed = parse(static_cast<IMC::Rows*>(msg), pos);
break;
case DUNE_IMC_YOYO:
parsed = parse(static_cast<IMC::YoYo*>(msg), pos);
break;
case DUNE_IMC_ELEVATOR:
parsed = parse(static_cast<IMC::Elevator*>(msg), pos);
break;
case DUNE_IMC_POPUP:
parsed = parse(static_cast<IMC::PopUp*>(msg), pos);
break;
case DUNE_IMC_COMPASSCALIBRATION:
parsed = parse(static_cast<IMC::CompassCalibration*>(msg), pos);
break;
default:
parsed = false;
break;
}
if (!parsed)
{
if (m_profiles.empty() || itr == nodes.begin())
return;
// return the duration from the previously computed maneuver
const_iterator dtr;
dtr = m_profiles.find((*(--itr))->maneuver_id);
if (dtr->second.durations.empty())
return;
m_last_valid = dtr->first;
return;
}
// Update speeds and durations
Profile prof;
prof.durations = m_accum_dur->vec;
prof.speeds = *m_speed_vec;
std::pair<std::string, Profile > p_pair((*itr)->maneuver_id, prof);
m_profiles.insert(p_pair);
}
Memory::clear(m_accum_dur);
Memory::clear(m_speed_vec);
m_last_valid = nodes.back()->maneuver_id;
m_finite_duration = true;
return;
}
