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);
}
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;
}
