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