void
Plan::fillTimeline(Timeline& tl)
{
float execution_duration = getExecutionDuration();
std::vector<IMC::PlanManeuver*>::const_iterator itr;
itr = m_seq_nodes.begin();
// Maneuver's start and end ETA
float maneuver_start_eta = -1.0;
float maneuver_end_eta = -1.0;
// Iterate through plan maneuvers
for (; itr != m_seq_nodes.end(); ++itr)
{
if (itr == m_seq_nodes.begin())
maneuver_start_eta = execution_duration;
else
maneuver_start_eta = maneuver_end_eta;
TimeProfile::const_iterator dur;
dur = m_profiles->find((*itr)->maneuver_id);
if (dur == m_profiles->end())
maneuver_end_eta = -1.0;
else if (dur->second.durations.size())
maneuver_end_eta = execution_duration - dur->second.durations.back();
else
maneuver_end_eta = -1.0;
// Fill timeline
tl.setManeuverETA((*itr)->maneuver_id, maneuver_start_eta, maneuver_end_eta);
}
}
void
Plan::secondaryParse(const std::map<std::string, IMC::EntityInfo>& cinfo,
IMC::PlanStatistics& ps, bool imu_enabled,
const IMC::EstimatedState* state)
{
// Pre statistics
ps.plan_id = m_spec->plan_id;
PreStatistics pre_stat(&ps);
if (m_compute_progress)
{
sequenceNodes();
if (isLinear() && state != NULL)
{
m_profiles->parse(m_seq_nodes, state);
Timeline tline;
fillTimeline(tline);
Memory::clear(m_sched);
m_sched = new ActionSchedule(m_task, m_spec, m_seq_nodes,
tline, cinfo);
// Update timeline with scheduled calibration time if any
tline.setPlanETA(std::max(m_sched->getEarliestSchedule(), getExecutionDuration()));
// Fill component active time with action scheduler
m_sched->fillComponentActiveTime(m_seq_nodes, tline, m_cat);
// Update duration statistics
pre_stat.fill(m_seq_nodes, tline);
// Update action statistics
pre_stat.fill(m_cat);
// Estimate necessary calibration time
float diff = m_sched->getEarliestSchedule() - getExecutionDuration();
m_est_cal_time = (uint16_t)std::max(0.0f, diff);
m_est_cal_time = (uint16_t)std::max(m_min_cal_time, m_est_cal_time);
if (m_predict_fuel)
{
Memory::clear(m_fpred);
m_fpred = new FuelPrediction(m_profiles, &m_cat, m_power_model,
m_speed_model, imu_enabled, tline.getPlanETA());
pre_stat.fill(*m_fpred);
}
}
else if (!isLinear())
{
Memory::clear(m_sched);
m_sched = new ActionSchedule(m_task, m_spec, m_seq_nodes, cinfo);
m_est_cal_time = m_min_cal_time;
}
}
if (!m_profiles->isDurationFinite())
m_properties |= IMC::PlanStatistics::PRP_INFINITE;
pre_stat.setProperties(m_properties);
}
float
Plan::progress(const IMC::ManeuverControlState* mcs)
{
if (!m_compute_progress)
return -1.0;
// Compute only if linear and durations exists
if (!isLinear() || !m_profiles->size())
return -1.0;
// If calibration has not started yet, but will later
if (m_calib->notStarted())
return -1.0;
float total_duration = getTotalDuration();
float exec_duration = getExecutionDuration();
// Check if its calibrating
if (m_calib->inProgress())
{
float time_left = m_calib->getRemaining() + exec_duration;
m_progress = 100.0 * trimValue(1.0 - time_left / total_duration, 0.0, 1.0);
return m_progress;
}
// If it's not executing, do not compute
if (mcs->state != IMC::ManeuverControlState::MCS_EXECUTING ||
mcs->eta == 0)
return m_progress;
TimeProfile::const_iterator itr;
itr = m_profiles->find(getCurrentId());
// If not found
if (itr == m_profiles->end())
{
// If beyond the last maneuver with valid duration
if (m_beyond_dur)
{
m_progress = 100.0;
return m_progress;
}
else
{
return -1.0;
}
}
// If durations vector for this maneuver is empty
if (!itr->second.durations.size())
return m_progress;
IMC::Message* man = m_graph.find(getCurrentId())->second.pman->data.get();
// Get execution progress
float exec_prog = Progress::compute(man, mcs, itr->second.durations, exec_duration);
float prog = 100.0 - getExecutionPercentage() * (1.0 - exec_prog / 100.0);
// If negative, then unable to compute
// But keep last value of progress if it is not invalid
if (prog < 0.0)
{
if (m_progress < 0.0)
return -1.0;
else
return m_progress;
}
// Never output shorter than previous
m_progress = prog > m_progress ? prog : m_progress;
return m_progress;
}
bool
Plan::parse(std::string& desc, const std::set<uint16_t>* supported_maneuvers,
bool plan_startup, const std::map<std::string, IMC::EntityInfo>& cinfo,
Tasks::Task* task, const IMC::EstimatedState* state)
{
bool start_maneuver_ok = false;
clear();
if (!m_spec->maneuvers.size())
{
desc = m_spec->plan_id + DTR(": no maneuvers");
return false;
}
IMC::MessageList<IMC::PlanManeuver>::const_iterator mitr;
mitr = m_spec->maneuvers.begin();
// parse maneuvers and transitions
do
{
if (*mitr == NULL)
{
++mitr;
continue;
}
if ((*mitr)->data.isNull())
{
desc = (*mitr)->maneuver_id + DTR(": actual maneuver not specified");
return false;
}
const IMC::Message* m = (*mitr)->data.get();
if (supported_maneuvers->find(m->getId()) == supported_maneuvers->end())
{
desc = (*mitr)->maneuver_id + DTR(": maneuver is not supported");
return false;
}
if ((*mitr)->maneuver_id == m_spec->start_man_id)
start_maneuver_ok = true;
Node node;
bool matched = false;
node.pman = (*mitr);
IMC::MessageList<IMC::PlanTransition>::const_iterator tritr;
tritr = m_spec->transitions.begin();
for (; tritr != m_spec->transitions.end(); ++tritr)
{
if (*tritr == NULL)
continue;
if ((*tritr)->dest_man == (*mitr)->maneuver_id)
matched = true;
if ((*tritr)->source_man == (*mitr)->maneuver_id)
node.trans.push_back((*tritr));
}
// if a match was not found and this is not the start maneuver
if (!matched && ((*mitr)->maneuver_id != m_spec->start_man_id))
{
std::string str = DTR(": maneuver has no incoming transition"
" and it's not the initial maneuver");
desc = (*mitr)->maneuver_id + str;
return false;
}
m_graph[(*mitr)->maneuver_id] = node;
++mitr;
}
while (mitr != m_spec->maneuvers.end());
if (!start_maneuver_ok)
{
desc = m_spec->start_man_id + DTR(": invalid start maneuver");
return false;
}
if (m_compute_progress && plan_startup)
{
sequenceNodes();
if (m_sequential && state != NULL)
{
computeDurations(state);
Memory::clear(m_sched);
m_sched = new ActionSchedule(task, m_spec, m_seq_nodes,
*m_durations, m_last_dur, cinfo);
// Estimate necessary calibration time
float diff = m_sched->getEarliestSchedule() - getExecutionDuration();
m_est_cal_time = (uint16_t)trimValue(diff, 0.0, diff);
m_est_cal_time = (uint16_t)std::max(m_min_cal_time, m_est_cal_time);
}
else if (!m_sequential)
{
Memory::clear(m_sched);
m_sched = new ActionSchedule(task, m_spec, m_seq_nodes, cinfo);
m_est_cal_time = m_min_cal_time;
}
}
m_last_id = m_spec->start_man_id;
return true;
}
