C++ Message::getId方法代码示例

void
GraphicsScene::handleInputData(void)
{
  while (m_sock->hasPendingDatagrams())
  {
    m_dgram.resize(m_sock->pendingDatagramSize());
    m_sock->readDatagram(m_dgram.data(), m_dgram.size());

    IMC::Message* msg = IMC::Packet::deserialize((uint8_t*)m_dgram.data(), m_dgram.size());
    if (msg == 0)
      continue;

    if (msg->getId() == DUNE_IMC_COMPRESSEDIMAGE)
    {
      ++m_fps;

      IMC::CompressedImage* img = static_cast<IMC::CompressedImage*>(msg);

      QPixmap pix;
      pix.loadFromData((uchar*)img->data.data(), img->data.size(), "JPEG");

      QTransform t;
      pix = pix.transformed(t.rotate(m_rotate));

      m_item.setPixmap(pix);

      setMinimumSize(pix.width() + c_pad, pix.height() + c_pad);

      if (m_grid)
      {
        m_vline->setLine(0, pix.height() / 2, pix.width(), pix.height() / 2);
        m_hline->setLine(pix.width() / 2, 0, pix.width() / 2, pix.height());
      }
    }
    else if (msg->getId() == DUNE_IMC_EULERANGLES)
    {
      IMC::EulerAngles* ang = static_cast<IMC::EulerAngles*>(msg);
      // QString str("Roll: %0.2f | Pitch: %0.2f");
      // m_text.setText(str.arg(Angles::degrees(ang->roll), Angles::degrees(ang->pitch)));
    }
  }
}

    void
    MessageMonitor::updateMessage(const IMC::Message* msg)
    {
      ScopedMutex l(m_mutex);

      IMC::Message* tmsg = msg->clone();
      unsigned key = tmsg->getId() << 24 | tmsg->getSubId() << 8 | tmsg->getSourceEntity();

      if (m_msgs[key])
        delete m_msgs[key];

      m_msgs[key] = tmsg;
    }

    void
    MessageMonitor::updateMessage(const IMC::Message* msg)
    {
      ScopedMutex l(m_mutex);

      if (msg->getId() == DUNE_IMC_POWERCHANNELSTATE)
        updatePowerChannel(static_cast<const IMC::PowerChannelState*>(msg));

      IMC::Message* tmsg = msg->clone();
      unsigned key = tmsg->getId() << 24 | tmsg->getSubId() << 8 | tmsg->getSourceEntity();

      if (m_msgs[key])
        delete m_msgs[key];

      m_msgs[key] = tmsg;
    }

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

int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " [options] <path_to_log_1/Data.lsf[.gz]> ... <path_to_log_n/Data.lsf[.gz]>"
              << std::endl
              << "Options:" << std::endl
              << "-i    Process idles. Idle logs are ignored by default."
              << std::endl;
    return 1;
  }

  std::ofstream csv("AllData.csv");
  csv << "timestamp (s), latitude (deg), longitude (deg), conductivity (S/m), temperature (ºC), depth (m)" << std::endl;

  for (int32_t i = 1; i < argc; ++i)
  {
    std::istream* is = 0;
    Compression::Methods method = Compression::Factory::detect(argv[i]);
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(argv[i], std::ios::binary);
    else
      is = new Compression::FileInput(argv[i], method);

    IMC::Message* msg = NULL;

    bool got_name = false;
    std::string log_name = "unknown";

    bool got_ent = false;
    unsigned ctd_ent = 0;

    bool ignore = false;

    RData rdata;
    rdata.clearData();
    std::stringstream ss_data;

    try
    {
      while ((msg = IMC::Packet::deserialize(*is)) != 0)
      {
        if (msg->getId() == DUNE_IMC_LOGGINGCONTROL)
        {
          IMC::LoggingControl* ptr = dynamic_cast<IMC::LoggingControl*>(msg);

          if (ptr->op == IMC::LoggingControl::COP_STARTED)
          {
            log_name = ptr->name;
            got_name = true;
          }
        }
        else if (msg->getId() == DUNE_IMC_ENTITYINFO)
        {
          IMC::EntityInfo *ptr = dynamic_cast<IMC::EntityInfo*>(msg);

          if (ptr->label.compare(c_ctd_label) == 0)
          {
            ctd_ent = ptr->id;
            got_ent = true;
          }
        }
        else if (msg->getId() == DUNE_IMC_ESTIMATEDSTATE)
        {
          IMC::EstimatedState* ptr = dynamic_cast<IMC::EstimatedState*>(msg);

          Coordinates::toWGS84(*ptr, rdata.lat, rdata.lon);
          rdata.timestamp = ptr->getTimeStamp();

          rdata.cnt |= GOT_STATE;
        }
        else if (rdata.gotState() && got_ent)
        {
          if (msg->getId() == DUNE_IMC_CONDUCTIVITY)
          {
            IMC::Conductivity* ptr = dynamic_cast<IMC::Conductivity*>(msg);
            if (ptr->getSourceEntity() == ctd_ent)
            {
              rdata.cond = ptr->value;
              rdata.cnt |= GOT_COND;
            }
          }
          else if (msg->getId() == DUNE_IMC_TEMPERATURE)
          {
            IMC::Temperature* ptr = dynamic_cast<IMC::Temperature*>(msg);
            if (ptr->getSourceEntity() == ctd_ent)
            {
              rdata.temp = ptr->value;
              rdata.cnt |= GOT_TEMP;
            }
          }
          else if (msg->getId() == DUNE_IMC_DEPTH)
          {
            IMC::Depth* ptr = dynamic_cast<IMC::Depth*>(msg);
            if (ptr->getSourceEntity() == ctd_ent)
            {
              rdata.depth = ptr->value;
              rdata.cnt |= GOT_DEPTH;
            }
//.........这里部分代码省略.........

int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " <path_to_log_1/Data.lsf[.gz]> ... <path_to_log_n/Data.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  std::map<std::string, Vehicle> vehicles;

  for (int32_t i = 1; i < argc; ++i)
  {
    std::istream* is = 0;
    Compression::Methods method = Compression::Factory::detect(argv[i]);
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(argv[i], std::ios::binary);
    else
      is = new Compression::FileInput(argv[i], method);

    IMC::Message* msg = NULL;

    uint16_t curr_rpm = 0;

    bool got_state = false;
    IMC::EstimatedState estate;
    double last_lat;
    double last_lon;

    // Accumulated travelled distance
    double distance = 0.0;
    // Accumulated travelled time
    double duration = 0.0;

    bool got_name = false;
    std::string log_name = "unknown";

    bool ignore = false;
    uint16_t sys_id = 0xffff;
    std::string sys_name;

    try
    {
      while ((msg = IMC::Packet::deserialize(*is)) != 0)
      {
        if (msg->getId() == DUNE_IMC_ANNOUNCE)
        {
          IMC::Announce* ptr = static_cast<IMC::Announce*>(msg);
          if (sys_id == ptr->getSource())
          {
            sys_name = ptr->sys_name;
          }
        }
        else if (msg->getId() == DUNE_IMC_LOGGINGCONTROL)
        {
          if (!got_name)
          {
            IMC::LoggingControl* ptr = static_cast<IMC::LoggingControl*>(msg);

            if (ptr->op == IMC::LoggingControl::COP_STARTED)
            {
              sys_id = ptr->getSource();
              log_name = ptr->name;
              got_name = true;
            }
          }
        }
        else if (msg->getId() == DUNE_IMC_ESTIMATEDSTATE)
        {
          if (msg->getTimeStamp() - estate.getTimeStamp() > c_timestep)
          {
            IMC::EstimatedState* ptr = static_cast<IMC::EstimatedState*>(msg);

            if (!got_state)
            {
              estate = *ptr;
              Coordinates::toWGS84(*ptr, last_lat, last_lon);

              got_state = true;
            }
            else if (curr_rpm > c_min_rpm)
            {
              double lat, lon;
              Coordinates::toWGS84(*ptr, lat, lon);

              double dist = Coordinates::WGS84::distance(last_lat, last_lon, 0.0,
                                                         lat, lon, 0.0);

              // Not faster than maximum considered speed
              if (dist / (ptr->getTimeStamp() - estate.getTimeStamp()) < c_max_speed)
              {
                distance += dist;
                duration += msg->getTimeStamp() - estate.getTimeStamp();
              }

              estate = *ptr;
              last_lat = lat;
              last_lon = lon;
            }
//.........这里部分代码省略.........

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

int
main(int32_t argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "Usage: " << argv[0] << " <abbrev of imc message 1>,<abbrev of imc message 2>,..,"
              << "<abbrev of imc message n> Data.lsf[.gz] .. Data.lsf[.gz]"
              << std::endl;
    std::cerr << argv[0] << " accepts multiple IMC messages comma separated and "
              << "multiple Data.lsf files space separated." << std::endl;
    std::cerr << "This program does not sort the input Data.lsf files." << std::endl;
    return 1;
  }

  ByteBuffer buffer;
  std::ofstream lsf("FilteredData.lsf", std::ios::binary);

  IMC::Message* msg;

  uint32_t accum = 0;

  bool done_first = false;

  std::set<uint32_t> ids;
  std::vector<std::string> msgs;
  Utils::String::split(argv[1], ",", msgs);

  for (unsigned k = 0; k < msgs.size(); ++k)
  {
    uint32_t got = IMC::Factory::getIdFromAbbrev(Utils::String::trim(msgs[k]));
    ids.insert(got);
  }

  for (uint32_t j = 2; j < (uint32_t)argc; ++j)
  {
    std::istream* is = 0;
    Compression::Methods method = Compression::Factory::detect(argv[j]);
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(argv[j], std::ios::binary);
    else
      is = new Compression::FileInput(argv[j], method);

    uint32_t i = 0;

    try
    {
      while ((msg = IMC::Packet::deserialize(*is)) != 0)
      {
        if (!done_first)
        {
          // place an empty estimatedstate message in the log
          IMC::EstimatedState state;
          state.setTimeStamp(msg->getTimeStamp());
          IMC::Packet::serialize(&state, buffer);
          lsf.write(buffer.getBufferSigned(), buffer.getSize());
          done_first = true;
        }

        std::set<uint32_t>::const_iterator it;
        it = ids.find(msg->getId());

        if (it != ids.end())
        {
          IMC::Packet::serialize(msg, buffer);
          lsf.write(buffer.getBufferSigned(), buffer.getSize());

          ++i;
        }

        delete msg;
      }
    }
    catch (std::runtime_error& e)
    {
      std::cerr << "ERROR: " << e.what() << std::endl;
      return -1;
    }

    std::cerr << i << " messages in " << argv[j] << std::endl;
    accum += i;

    delete is;
  }

  lsf.close();

  std::cerr << "Total of " << accum << " " << argv[1] << " messages." << std::endl;

  return 0;
}

int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " <path_to_log/Data.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  std::istream* is = 0;
  Compression::Methods method = Compression::Factory::detect(argv[1]);
  if (method == METHOD_UNKNOWN)
    is = new std::ifstream(argv[1], std::ios::binary);
  else
    is = new Compression::FileInput(argv[1], method);

  IMC::Message* msg = NULL;

  unsigned phototrigger_eid = 0;

  std::ofstream logfile;
  logfile.open("gps.txt", std::ofstream::app);

  try
  {
    while ((msg = IMC::Packet::deserialize(*is)) != 0)
    {
      if (msg->getId() == DUNE_IMC_LOGBOOKENTRY)
      {
        IMC::LogBookEntry* lbe = static_cast<IMC::LogBookEntry*>(msg);

        if (lbe->getSourceEntity() != phototrigger_eid)
          continue;

        double lat;
        double lon;
        double height;

        std::sscanf(lbe->text.c_str(), "%lf,%lf,%lf", &lat, &lon, &height);

        logfile << std::setprecision(10) << Angles::degrees(lat) << "," << Angles::degrees(lon) << "," << height << std::endl;
      }

      else if (msg->getId() == DUNE_IMC_ENTITYINFO)
      {
        IMC::EntityInfo* ei = static_cast<IMC::EntityInfo*>(msg);

        if (!std::strcmp(ei->label.c_str(), "Photo Trigger"))
          phototrigger_eid = ei->id;
      }

      delete msg;
    }

    logfile.close();
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
  }

  return 0;
}

//.........这里部分代码省略.........
    // Last device turned on
    Id2Name::const_iterator last_device = channel_names.end();

    try
    {
        while ((msg = IMC::Packet::deserialize(*is)) != 0)
        {
            if (last_timestamp < 0.0)
                last_timestamp = msg->getTimeStamp();

            if (ignore_data != was_ignoring)
            {
                if (!was_ignoring)
                {
                    // reset timer counter
                    counter = 0.0;
                }
            }

            if (ignore_data && test_started)
            {
                counter += msg->getTimeStamp() - last_timestamp;

                if (counter >= ignore_timeout)
                {
                    counter = 0.0;
                    ignore_data = false;
                }
            }

            was_ignoring = ignore_data;
            last_timestamp = msg->getTimeStamp();

            if (msg->getId() == DUNE_IMC_ENTITYINFO)
            {
                IMC::EntityInfo* einfo = dynamic_cast<IMC::EntityInfo*>(msg);

                for (unsigned i = 0; i < SU_COUNT; i++)
                {
                    if (std::strcmp(einfo->label.c_str(), c_supply_name[i].c_str()) == 0)
                    {
                        measure_ids.insert(std::pair<uint8_t, uint8_t>(einfo->id, i));
                        break;
                    }
                }
            }
            else if (msg->getId() == DUNE_IMC_POWERCHANNELCONTROL)
            {
                IMC::PowerChannelControl* pcc = dynamic_cast<IMC::PowerChannelControl*>(msg);

                if (pcc->op == IMC::PowerChannelControl::PCC_OP_TURN_OFF && got_channels)
                {
                    ignore_data = true;
                    test_started = true;
                }
                else if (pcc->op == IMC::PowerChannelControl::PCC_OP_TURN_ON)
                {
                    if (last_device != channel_names.end())
                    {
                        printPower(last_device->second, voltage_measures, current_measures);
                    }
                    else
                    {
                        for (unsigned i = 0; i < SU_COUNT; i++)
                        {
                            voltage_measures[i].update();

int
main(int32_t argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "Usage: " << argv[0] << " Data.lsf[.gz]"
              << std::endl;
    return 1;
  }

  std::istream* is = 0;
  Compression::Methods method = Compression::Factory::detect(argv[1]);
  if (method == METHOD_UNKNOWN)
    is = new std::ifstream(argv[1], std::ios::binary);
  else
    is = new Compression::FileInput(argv[1], method);

  ByteBuffer buffer;
  std::ofstream lsf("SurfaceData.lsf", std::ios::binary);

  IMC::Message* msg;

  unsigned i = 0;

  IMC::EstimatedState state;
  IMC::Packet::serialize(&state, buffer);
  lsf.write(buffer.getBufferSigned(), buffer.getSize());
  double timestamp = -1.0;

  try
  {
    while ((msg = IMC::Packet::deserialize(*is)) != 0)
    {
      if (msg->getId() == DUNE_IMC_GPSFIX)
      {
        IMC::GpsFix* fix = static_cast<IMC::GpsFix*>(msg);

        if ((fix->hacc <= MIN_HACC) &&
            (fix->validity & IMC::GpsFix::GFV_VALID_POS) &&
            (fix->getTimeStamp() >= timestamp))
        {
          timestamp = fix->getTimeStamp();

          IMC::Packet::serialize(msg, buffer);
          lsf.write(buffer.getBufferSigned(), buffer.getSize());

          ++i;
        }
      }

      delete msg;
    }
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return -1;
  }

  lsf.close();

  delete is;

  std::cerr << "Got " << i << " GpsFix messages." << std::endl;

  return 0;
}

int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " <path_to_log/Data.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  std::istream* is = 0;
  Compression::Methods method = Compression::Factory::detect(argv[1]);
  if (method == METHOD_UNKNOWN)
    is = new std::ifstream(argv[1], std::ios::binary);
  else
    is = new Compression::FileInput(argv[1], method);

  IMC::Message* msg = NULL;

  std::ofstream lsf("Data.lsf", std::ios::binary);

  DUNE::Utils::ByteBuffer buffer;

  float bottom_follow_depth = -1.0;
  float vertical_ref = -1.0;

  // Control parcel for debug
  IMC::ControlParcel parcel;

  // Last EstimatedState
  IMC::EstimatedState last_state;
  bool got_state = false;

  // Coarse altitude control
  DUNE::Control::CoarseAltitude::Arguments args;
  createCA(&args);
  DUNE::Control::CoarseAltitude ca(&args);

  try
  {
    while ((msg = IMC::Packet::deserialize(*is)) != 0)
    {
      if (msg->getId() == DUNE_IMC_ESTIMATEDSTATE)
      {
        IMC::EstimatedState* state = dynamic_cast<IMC::EstimatedState*>(msg);

        if (!got_state)
        {
          last_state = *state;
          got_state = true;
        }
        else
        {
          IMC::Packet::serialize(state, buffer);
          lsf.write(buffer.getBufferSigned(), buffer.getSize());
          buffer.resetBuffer();

          if (bottom_follow_depth > 0.0)
          {
            bottom_follow_depth = state->depth + (state->alt - vertical_ref);
            parcel.p = bottom_follow_depth;
            parcel.i = ca.update(state->getTimeStamp() - last_state.getTimeStamp(),
                                 state->depth, bottom_follow_depth);
            parcel.d = state->depth - bottom_follow_depth;
            // parcel.a = state->depth - parcel.i;
            parcel.a = ca.getCorridor();

            parcel.setTimeStamp(state->getTimeStamp());

            IMC::Packet::serialize(&parcel, buffer);
            lsf.write(buffer.getBufferSigned(), buffer.getSize());
            buffer.resetBuffer();
          }

          last_state = *state;
        }
      }
      else if (msg->getId() == DUNE_IMC_DESIREDZ)
      {
        IMC::DesiredZ* ptr = dynamic_cast<IMC::DesiredZ*>(msg);

        if (ptr->z_units == IMC::Z_ALTITUDE)
        {
          vertical_ref = ptr->value;
          bottom_follow_depth = last_state.depth;
        }

        IMC::Packet::serialize(ptr, buffer);
        lsf.write(buffer.getBufferSigned(), buffer.getSize());
        buffer.resetBuffer();
      }
      else if (msg->getId() == DUNE_IMC_LOGGINGCONTROL)
      {
        IMC::LoggingControl* ptr = dynamic_cast<IMC::LoggingControl*>(msg);
        IMC::Packet::serialize(ptr, buffer);
        lsf.write(buffer.getBufferSigned(), buffer.getSize());
        buffer.resetBuffer();
      }

      delete msg;
//.........这里部分代码省略.........

int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " <path_to_log_1/Data.lsf[.gz]> ... <path_to_log_n/Data.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  double total_accum = 0;

  for (int32_t i = 1; i < argc; ++i)
  {
    std::istream* is = 0;
    Compression::Methods method = Compression::Factory::detect(argv[i]);
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(argv[i], std::ios::binary);
    else
      is = new Compression::FileInput(argv[i], method);

    IMC::Message* msg = NULL;

    uint16_t curr_rpm = 0;

    bool got_state = false;
    IMC::EstimatedState estate;
    double last_lat;
    double last_lon;

    // Accumulated travelled distance
    double accum = 0;

    bool got_name = false;
    std::string log_name = "unknown";

    bool ignore = false;

    try
    {
      while ((msg = IMC::Packet::deserialize(*is)) != 0)
      {

        if (msg->getId() == DUNE_IMC_LOGGINGCONTROL)
        {
          if (!got_name)
          {
            IMC::LoggingControl* ptr = dynamic_cast<IMC::LoggingControl*>(msg);

            if (ptr->op == IMC::LoggingControl::COP_STARTED)
            {
              log_name = ptr->name;
              got_name = true;
            }
          }
        }
        else if (msg->getId() == DUNE_IMC_ESTIMATEDSTATE)
        {
          if (msg->getTimeStamp() - estate.getTimeStamp() > c_timestep)
          {
            IMC::EstimatedState* ptr = dynamic_cast<IMC::EstimatedState*>(msg);

            if (!got_state)
            {
              estate = *ptr;
              Coordinates::toWGS84(*ptr, last_lat, last_lon);

              got_state = true;
            }
            else if (curr_rpm > c_min_rpm)
            {
              double lat, lon;
              Coordinates::toWGS84(*ptr, lat, lon);

              double dist = Coordinates::WGS84::distance(last_lat, last_lon, 0.0,
                                                         lat, lon, 0.0);

              // Not faster than maximum considered speed
              if (dist / (ptr->getTimeStamp() - estate.getTimeStamp()) < c_max_speed)
                accum += dist;

              estate = *ptr;
              Coordinates::toWGS84(*ptr, last_lat, last_lon);
            }
          }
        }
        else if (msg->getId() == DUNE_IMC_RPM)
        {
          IMC::Rpm* ptr = dynamic_cast<IMC::Rpm*>(msg);
          curr_rpm = ptr->value;
        }
        else if (msg->getId() == DUNE_IMC_SIMULATEDSTATE)
        {
          // since it has simulated state let us ignore this log
          ignore = true;
          delete msg;
          std::cerr << "this is a simulated log";
          break;
        }

//.........这里部分代码省略.........

    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;
//.........这里部分代码省略.........