Пример #1
0
      void
      onMain(void)
      {
        setInitialState();

        while (!stopping())
        {
          if (m_report_timer.overflow())
          {
            if (m_args.progress)
              reportProgress();

            dispatch(m_pcs);

            m_report_timer.reset();
          }

          double now = Clock::get();

          if ((getEntityState() == IMC::EntityState::ESTA_NORMAL) &&
              (now - m_last_vstate >= c_vs_timeout))
          {
            changeMode(IMC::PlanControlState::PCS_BLOCKED, DTR("vehicle state timeout"));
            m_last_vstate = now;
          }

          // got requests to process
          if (!pendingReply() && m_requests.size())
          {
            processRequest(&m_requests.front());
            m_requests.pop();
          }

          double delta = m_vc_reply_deadline < 0 ? 1 : m_vc_reply_deadline - now;

          if (delta > 0)
          {
            waitForMessages(std::min(1.0, delta));
            continue;
          }

          // handle reply timeout
          m_vc_reply_deadline = -1;

          changeMode(IMC::PlanControlState::PCS_READY, DTR("vehicle reply timeout"));

          // Popping all requests
          while (m_requests.size())
            m_requests.pop();

          // Increment local request id to prevent old replies from being processed
          ++m_vreq_ctr;

          err(DTR("cleared all requests"));
        }
      }
Пример #2
0
      void
      consume(const IMC::VehicleState* vs)
      {
        if (getEntityState() == IMC::EntityState::ESTA_BOOT)
          return;

        m_last_vstate = Clock::get();

        switch (vs->op_mode)
        {
          case IMC::VehicleState::VS_SERVICE:
            onVehicleService(vs);
            break;
          case IMC::VehicleState::VS_CALIBRATION:
            onVehicleCalibration(vs);
            break;
          case IMC::VehicleState::VS_ERROR:
          case IMC::VehicleState::VS_BOOT:
            onVehicleError(vs);
            break;
          case IMC::VehicleState::VS_MANEUVER:
            onVehicleManeuver(vs);
            break;
          case IMC::VehicleState::VS_EXTERNAL:
            onVehicleExternalControl(vs);
            break;
        }

        // update calibration status
        if (m_plan != NULL && initMode())
        {
          m_plan->updateCalibration(vs);

          if (m_plan->isCalibrationDone())
          {
            if ((vs->op_mode == IMC::VehicleState::VS_CALIBRATION) &&
                !pendingReply())
            {
              IMC::PlanManeuver* pman = m_plan->loadStartManeuver();
              startManeuver(pman);
            }
          }
          else if (m_plan->hasCalibrationFailed())
          {
            onFailure(m_plan->getCalibrationInfo());
            m_reply.plan_id = m_spec.plan_id;
            changeMode(IMC::PlanControlState::PCS_READY, m_plan->getCalibrationInfo());
          }
        }
      }
Пример #3
0
      void
      onMain(void)
      {
        setInitialState();

        while (!stopping())
        {
          if (m_report_timer.overflow())
          {
            if (m_args.progress)
              reportProgress();

            dispatch(m_pcs);

            m_report_timer.reset();
          }

          double now = Clock::get();

          if ((getEntityState() == IMC::EntityState::ESTA_NORMAL) &&
              (now - m_last_vstate >= c_vs_timeout))
          {
            changeMode(IMC::PlanControlState::PCS_BLOCKED, DTR("vehicle state timeout"));
            m_last_vstate = now;
          }

          double delta = m_vc_reply_deadline < 0 ? 1 : m_vc_reply_deadline - now;

          if (delta > 0)
          {
            waitForMessages(std::min(1.0, delta));
            continue;
          }

          // handle reply timeout
          m_vc_reply_deadline = -1;

          changeMode(IMC::PlanControlState::PCS_READY, DTR("vehicle reply timeout"));
        }
      }
Пример #4
0
      void
      processRequest(const IMC::PlanControl* pc)
      {
        m_reply.setDestination(pc->getSource());
        m_reply.setDestinationEntity(pc->getSourceEntity());
        m_reply.request_id = pc->request_id;
        m_reply.op = pc->op;
        m_reply.plan_id = pc->plan_id;

        inf(DTR("request -- %s (%s)"),
            DTR(c_op_desc[m_reply.op]),
            m_reply.plan_id.c_str());

        if (getEntityState() != IMC::EntityState::ESTA_NORMAL)
        {
          onFailure(DTR("engine not ready: entity state not normal"));
          return;
        }

        switch (pc->op)
        {
          case IMC::PlanControl::PC_START:
            if (!startPlan(pc->plan_id, pc->arg.isNull() ? 0 : pc->arg.get(), pc->flags))
              vehicleRequest(IMC::VehicleCommand::VC_STOP_MANEUVER);
            break;
          case IMC::PlanControl::PC_STOP:
            stopPlan();
            break;
          case IMC::PlanControl::PC_LOAD:
            loadPlan(pc->plan_id, pc->arg.isNull() ? 0 : pc->arg.get(), false);
            break;
          case IMC::PlanControl::PC_GET:
            getPlan();
            break;
          default:
            onFailure(DTR("plan control operation not supported"));
            break;
        }
      }