Example #1
0
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;
            }
          }
        }
        else if (msg->getId() == DUNE_IMC_RPM)
        {
          IMC::Rpm* ptr = static_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;
        }

        delete msg;

        // ignore idles
        // either has the string _idle or has only the time.
        if (log_name.find("_idle") != std::string::npos ||
            log_name.size() == 15)
        {
          ignore = true;
          std::cerr << "this is an idle log";
          break;
        }
      }
    }
    catch (std::runtime_error& e)
    {
      std::cerr << "ERROR: " << e.what() << std::endl;
    }

    delete is;

    if (ignore)
    {
      std::cerr << "... ignoring" << std::endl;
      continue;
    }

    if (distance > 0)
    {
      vehicles[sys_name].duration += duration;
      vehicles[sys_name].distance += distance;
      vehicles[sys_name].logs.push_back(Log(log_name, distance, duration));
    }
  }

  double total_distance = 0;
  double total_duration = 0;

  std::map<std::string, Vehicle>::const_iterator itr = vehicles.begin();
  for (; itr != vehicles.end(); ++itr)
  {
    std::cout << std::endl;
    std::cout << "## " << itr->first << std::endl << std::endl;
    std::cout << "* Distance travelled per plan (m):" << std::endl;

    for (size_t i = 0; i < itr->second.logs.size(); ++i)
    {
      std::cout << " - "
                << itr->second.logs[i].distance
                << " in " << String::replace(itr->second.logs[i].name, '_', "\\_")
                << "." << std::endl;
    }

    total_distance += itr->second.distance;
    total_duration += itr->second.duration;

    std::cout << std::endl
              << "* Total travelled distance:" << std::endl
              << " - "
              << std::setprecision(4)
              << std::fixed
              << itr->second.distance / 1000.0 << " km / "
              << (unsigned)itr->second.duration / 60 / 60 << " h "
              << (unsigned)(itr->second.duration / 60) % 60 << " m "
              << (unsigned)itr->second.duration % 60 << " s" << "." << std::endl;
  }

  if (vehicles.size() > 1)
  {
    std::cout << std::endl
              << "## Summary" << std::endl
              << " - Total distance: "
              << std::setprecision(2)
              << std::fixed
              << total_distance / 1000.0 << " km" << std::endl
              << " - Total duration: "
              << (unsigned)total_duration / 60 / 60 << " h "
              << (unsigned)(total_duration / 60) % 60 << " m "
              << (unsigned)total_duration % 60 << " s" << std::endl;
  }

  return 0;
}
Example #2
0
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;
}
Example #3
0
int
main(int argc, char** argv)
{
  double speed = 1, begin = 0, end = -1;
  std::map<std::string, bool> filter;
  bool filtering = false;
  int verbose = 0;
  uint16_t src = 0xFFFF, dst = 0xFFFF;

  ++argv; --argc;

  if (!argc)
  {
    usage();
    return 1;
  }

  for (; *argv && **argv == '-'; ++argv, --argc)
  {
    char opt = (*argv)[1];
    ++argv; --argc;

    if (!*argv || **argv == '-')
    {
      std::cerr << "Invalid options\n";
      usage();
      return 1;
    }

    // @todo Use DUNE's OptionParser, too lazy now to do it.
    switch (opt)
    {
      case 'b':
      {
        char* aux;
        begin = std::strtod(*argv, &aux);
        if (*aux != 0 || begin < 0)
        {
          std::cerr << "Invalid begin time: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'e':
      {
        char* aux;
        end = std::strtod(*argv, &aux);
        if (*aux != 0 || end < 0)
        {
          std::cerr << "Invalid end time: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }

      case 'S':
      {
        char* aux;
        src = std::strtol(*argv, &aux, 10);
        if (*aux != 0)
        {
          std::cerr << "Invalid source address: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'D':
      {
        char* aux;
        dst = std::strtol(*argv, &aux, 10);
        if (*aux != 0)
        {
          std::cerr << "Invalid destination adress: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 's':
      {
        char* aux;
        speed = std::strtod(*argv, &aux);
        if (*aux != 0 || speed < 0)
        {
          std::cerr << "Invalid speed setting: " << *argv << '\n';
          usage();
          return 1;
        }
        break;
      }
      case 'v':
        verbose = std::atoi(*argv);
        break;
      case 'm':
      {
        std::vector<std::string> list;
        DUNE::Utils::String::split(*argv, ",", list);
        for (uint16_t i = 0; i < list.size(); ++i)
          filter[list[i]] = true;
        filtering = true;
      }
      break;
      default:
        std::cerr << "Invalid option: '-" << opt << "\'\n";
        usage();
        return 1;
    }
  }

  if (argc < 3)
  {
    std::cerr << "Invalid arguments" << std::endl;
    usage();
    return 1;
  }

  if (begin > 0 && end > 0 && begin > end)
  {
    std::cerr << "Invalid time offsets" << std::endl;
    usage();
    return 1;
  }

  UDPSocket sock;
  Address dest(argv[0]);
  uint16_t port = std::atoi(argv[1]);

  argv += 2;

  std::cout << std::fixed << std::setprecision(4);

  for (; *argv != 0; argv++)
  {
    Path file(*argv);
    std::istream* is;

    if (file.isDirectory())
    {
      file = file / "Data.lsf";
      if (!file.isFile())
        file += ".gz";
    }

    if (!file.isFile())
    {
      std::cerr << file << " does not exist\n";
      return 1;
    }

    Compression::Methods method = Compression::Factory::detect(file.c_str());
    if (method == METHOD_UNKNOWN)
      is = new std::ifstream(file.c_str(), std::ios::binary);
    else
      is = new Compression::FileInput(file.c_str(), method);

    IMC::Message* m;

    m = IMC::Packet::deserialize(*is);
    if (!m)
    {
      std::cerr << file << " contains no messages\n";
      delete is;
      continue;
    }

    DUNE::Utils::ByteBuffer bb;

    double time_origin = m->getTimeStamp();
    if (begin >= 0)
    {
      do
      {
        if (m->getTimeStamp() - time_origin >= begin)
          break;
        delete m;
        m = IMC::Packet::deserialize(*is);
      }
      while (m);

      if (!m)
      {
        std::cerr << "no messages for specified time range" << std::endl;
        return 1;
      }
    }
    else
      begin = 0;

    double start_time = Clock::getSinceEpoch();
    double now = start_time;

    do
    {
      double msg_ts = m->getTimeStamp();
      double vtime = msg_ts - time_origin;

      m->setTimeStamp(start_time + vtime);

      double future = 0;

      if (speed > 0 && vtime >= begin)
      {
        // Delay time to mimic behavior at specified speed
        future = start_time + vtime / speed - begin;
        double delay_time = (future - now);
        if (delay_time > 0)
          Delay::wait(delay_time);
      }
      now = Clock::getSinceEpoch();

      if (vtime >= begin
          && (src == 0xFFFF || src == m->getSource())
          && (dst == 0xFFFF || dst == m->getDestination())
          && (!filtering || filter[m->getName()]))
      {
        // Send message
        IMC::Packet::serialize(m, bb);
        sock.write(bb.getBuffer(), m->getSerializationSize(), dest, port);
        if (verbose >= 1)
          std::cout << (begin + now - start_time) << ' ' << vtime << ' ' << now - future << " : " << m->getName() << '\n';
        if (verbose >= 2)
          m->toText(std::cout);
      }

      delete m;

      if (end >= 0 && vtime >= end)
        break;
    }
    while ((m = IMC::Packet::deserialize(*is)) != 0);
    delete is;
  }
  return 0;
}
Example #4
0
int
main(int argc, char** argv)
{
    Utils::OptionParser options;
    options.executable(argv[0])
    .program(argv[0])
    .copyright(DUNE_COPYRIGHT)
    .email(DUNE_CONTACT)
    .version("1.0")
    .date(DUNE_BUILD_TIME)
    .arch(DUNE_SYSTEM_NAME)
    .add("-t", "--timeout",
         "Interval to ignore data right after a new device has been turned on."
         " Default is 10.0", "TIMEOUT")
    .add("-f", "--file",
         "Log file in .lsf or .lsf.gz format", "FILE");

    // Parse command line arguments.
    if (!options.parse(argc, argv) || options.value("--file").empty())
    {
        if (options.bad())
            std::cerr << "ERROR: " << options.error() << std::endl;

        options.usage();
        return 1;
    }

    std::string file = options.value("--file");

    std::istream* is = 0;
    Compression::Methods method = Compression::Factory::detect(file.c_str());
    if (method == METHOD_UNKNOWN)
        is = new std::ifstream(file.c_str(), std::ios::binary);
    else
        is = new Compression::FileInput(file.c_str(), method);

    IMC::Message* msg = NULL;

    // Current and voltage measurements
    ElectricMeasure current_measures[SU_COUNT];
    ElectricMeasure voltage_measures[SU_COUNT];

    typedef std::map<uint8_t, std::string> Id2Name;
    Id2Name channel_names;

    typedef std::map<uint8_t, uint8_t> Supply2Id;
    Supply2Id measure_ids;

    typedef std::map<std::string, double> Name2Power;
    Name2Power device_power;

    // Got all power channels
    bool got_channels = false;

    // Time counter
    double counter = 0.0;

    // Test has started
    bool test_started = false;

    // Ignore measures if true
    bool ignore_data = true;
    bool was_ignoring = true;

    // Last timestamp
    double last_timestamp = -1.0;

    // Timeout for ignoring data
    double ignore_timeout;
    if (options.value("--timeout").empty())
        ignore_timeout = 10.0;
    else
        ignore_timeout = atoi(options.value("--timeout").c_str());

    // 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();
                            current_measures[i].update();
                        }
                    }

                    ignore_data = true;
                    last_device = channel_names.find(pcc->id);
                }
            }
            else if (msg->getId() == DUNE_IMC_POWERCHANNELSTATE)
            {
                IMC::PowerChannelState* pcs = dynamic_cast<IMC::PowerChannelState*>(msg);

                if (channel_names.find(pcs->id) == channel_names.end())
                    channel_names.insert(std::pair<uint8_t, std::string>(pcs->id, pcs->label));
                else
                    got_channels = true;
            }
            else if (msg->getId() == DUNE_IMC_VOLTAGE)
            {
                IMC::Voltage* volt = dynamic_cast<IMC::Voltage*>(msg);
                Supply2Id::const_iterator it = measure_ids.find(volt->getSourceEntity());

                if (it != measure_ids.end() && !ignore_data)
                    voltage_measures[it->second].addMeasure(volt->value);
            }
            else if (msg->getId() == DUNE_IMC_CURRENT)
            {
                IMC::Current* curr = dynamic_cast<IMC::Current*>(msg);
                Supply2Id::const_iterator it = measure_ids.find(curr->getSourceEntity());

                if (it != measure_ids.end() && !ignore_data)
                    current_measures[it->second].addMeasure(curr->value);
            }
        }

        // Compute last device in the list
        printPower(last_device->second, voltage_measures, current_measures);
    }
    catch (std::runtime_error& e)
    {
        std::cerr << "ERROR: " << e.what() << std::endl;
    }

    return 0;
}
Example #5
0
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;
        }

        delete msg;

        // ignore idles
        // either has the string _idle or has only 
        if (log_name.find("_idle") != std::string::npos ||
            log_name.size() == 15)
        {
          ignore = true;
          std::cerr << "this is an idle log";
          break;
        }
      }
    }
    catch (std::runtime_error& e)
    {
      std::cerr << "ERROR: " << e.what() << std::endl;
    }

    delete is;

    if (ignore)
    {
      std::cerr << "... ignoring" << std::endl;
      continue;
    }

    std::cerr << "Travelled " << accum << " in " << log_name << "." << std::endl;

    total_accum += accum;
  }

  std::cerr << "Total travelled distance is " << total_accum << "m" << std::endl
            << " or " << total_accum / 1000.0 << " km." << std::endl;

  return 0;
}