Example #1
0
 //! Dispatch message to the message bus in reply to another
 //! message.
 //! @param[in] original original message.
 //! @param[in] msg message reference.
 //! @param[in] flags bitfield with flags (see Tasks::DispatchFlags).
 void
 dispatchReply(const IMC::Message& original, IMC::Message& msg, unsigned int flags = 0)
 {
   msg.setDestination(original.getSource());
   msg.setDestinationEntity(original.getSourceEntity());
   dispatch(msg, flags);
 }
Example #2
0
static void
sendMsg(IMC::Message& msg, UDPSocket& sock, Address& dest, int port)
{
    DUNE::Utils::ByteBuffer bb;
    msg.setTimeStamp();
    IMC::Packet::serialize(&msg, bb);
    sock.write((const uint8_t*)bb.getBuffer(), msg.getSerializationSize(), dest, port);
    msg.toText(std::cout);
}
Example #3
0
    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;
    }
Example #4
0
File: Task.cpp Project: YoncaB/dune
      void
      task(void)
      {
        // Return if task is not active.
        if (!isActive())
          return;

        double slat, slon, dx, dy, dz;
        slat = m_last_state.lat;
        slon = m_last_state.lon;

        // get absolute (simulated) position
        WGS84::displace(m_last_state.x, m_last_state.y, &slat, &slon);

        // compute offset from plume peak
        WGS84::displacement(slat, slon, 0, m_args.peak_lat, m_args.peak_lon, 0, &dx, &dy, &dz);

        // calculate value based on 2d gaussian function
        double expn = exp(-1 * ((dx * dx + dy * dy)
                                /(2 * m_args.peak_width * m_args.peak_width)));

        double val = m_args.away_val + (m_args.peak_val - m_args.away_val) * expn;
        val += m_prng->gaussian() * m_args.std_dev;
        m_msg->setValueFP(val);
        dispatch(m_msg);
      }
Example #5
0
void
sendMessage(IMC::Message& msg)
{
  uint16_t rv = IMC::Packet::serialize(&msg, (uint8_t*)g_buffer, sizeof(g_buffer));
  g_sock.write(g_buffer, rv, g_addr, g_port);
  msg.toText(std::cerr);
}
Example #6
0
    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;
    }
Example #7
0
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)));
    }
  }
}
Example #8
0
int
main(int argc, char** argv)
{
  if (argc < 4)
  {
    fprintf(stderr, "Usage: %s <destination host> <destination port> <abbrev> [arguments]\n", argv[0]);
    return 1;
  }

  Address dest(argv[1]);

  // Parse port.
  unsigned port = 0;
  if (!castLexical(argv[2], port))
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  if (port > 65535)
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  IMC::Message* msg = NULL;

  if (strcmp(argv[3], "Heartbeat") == 0)
  {
    IMC::Heartbeat* tmsg = new IMC::Heartbeat;
    msg = tmsg;
  }
  if (strcmp(argv[3], "RestartSystem") == 0)
  {
    IMC::RestartSystem* tmsg = new IMC::RestartSystem;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "Sms") == 0)
  {
    IMC::Sms* tmsg = new IMC::Sms;
    tmsg->number = argv[4];
    tmsg->timeout = atoi(argv[5]);
    tmsg->contents = argv[6];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "EntityState") == 0)
  {
    IMC::EntityState* tmsg = new IMC::EntityState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "EntityActivationState") == 0)
  {
    IMC::EntityActivationState* tmsg = new IMC::EntityActivationState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = IMC::EntityActivationState::EAS_ACTIVE;
  }
  else if (strcmp(argv[3], "MagneticField") == 0)
  {
    IMC::MagneticField* tmsg = new IMC::MagneticField;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->x = atof(argv[5]);
    tmsg->y = atof(argv[6]);
    tmsg->z = atof(argv[7]);
  }
  else if (strcmp(argv[3], "DataSanity") == 0)
  {
    IMC::DataSanity* tmsg = new IMC::DataSanity;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->sane = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "MonitorEntityState") == 0)
  {
    IMC::MonitorEntityState* tmsg = new IMC::MonitorEntityState;
    msg = tmsg;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
  }
  else if (strcmp(argv[3], "Abort") == 0)
  {
    msg = new IMC::Abort;
  }
  else if (strcmp(argv[3], "LoggingControl") == 0)
  {
    IMC::LoggingControl* tmsg = new IMC::LoggingControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
    tmsg->name = argv[5];
  }
  else if (strcmp(argv[3], "CacheControl") == 0)
  {
    IMC::CacheControl* tmsg = new IMC::CacheControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "LblRange") == 0)
  {
    IMC::LblRange* tmsg = new IMC::LblRange;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->range = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "LblConfig") == 0)
  {
    IMC::LblConfig* tmsg = new IMC::LblConfig;
    msg = tmsg;
    tmsg->op = IMC::LblConfig::OP_SET_CFG;

    IMC::LblBeacon bc;
    bc.beacon = "b2";
    bc.lat = 0.71883274;
    bc.lon = -0.15194732;
    bc.depth = 3;
    bc.query_channel = 4;
    bc.reply_channel = 5;
    bc.transponder_delay = 0;
    tmsg->beacons.push_back(bc);
    bc.beacon = "b3";
    bc.lat = 0.71881068;
    bc.lon = -0.15192335;
    bc.reply_channel = 6;
    tmsg->beacons.push_back(bc);
  }
  else if (strcmp(argv[3], "DesiredZ") == 0)
  {
    IMC::DesiredZ* tmsg = new IMC::DesiredZ;
    tmsg->value = atof(argv[4]);
    tmsg->z_units = static_cast<IMC::ZUnits>(atoi(argv[5]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredPitch") == 0)
  {
    IMC::DesiredPitch* tmsg = new IMC::DesiredPitch;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "Calibration") == 0)
  {
    IMC::Calibration* tmsg = new IMC::Calibration;
    tmsg->duration = (uint16_t)(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredHeading") == 0)
  {
    IMC::DesiredHeading* tmsg = new IMC::DesiredHeading;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredHeadingRate") == 0)
  {
    IMC::DesiredHeadingRate* tmsg = new IMC::DesiredHeadingRate;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredSpeed") == 0)
  {
    IMC::DesiredSpeed* tmsg = new IMC::DesiredSpeed;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->speed_units = IMC::SUNITS_PERCENTAGE;
    else
      tmsg->speed_units = atoi(argv[5]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "DesiredControl") == 0)
  {
    IMC::DesiredControl* tmsg = new IMC::DesiredControl;
    tmsg->k = atof(argv[4]);
    tmsg->m = atof(argv[5]);
    tmsg->n = atof(argv[6]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "SetThrusterActuation") == 0)
  {
    IMC::SetThrusterActuation* tmsg = new IMC::SetThrusterActuation;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }
  else if (strcmp(argv[3], "SetServoPosition") == 0)
  {
    IMC::SetServoPosition* tmsg = new IMC::SetServoPosition;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }
  else if (strcmp(argv[3], "GpsFix") == 0)
  {
    IMC::GpsFix* tmsg = new IMC::GpsFix;
    msg = tmsg;
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->height = atof(argv[6]);
  }
  else if (strcmp(argv[3], "VehicleCommand") == 0)
  {
    IMC::VehicleCommand* tmsg = new IMC::VehicleCommand;
    msg = tmsg;
    tmsg->type = IMC::VehicleCommand::VC_REQUEST;
    tmsg->command = atoi(argv[4]);

    if (tmsg->command == IMC::VehicleCommand::VC_EXEC_MANEUVER)
      tmsg->maneuver.set(dynamic_cast<IMC::Maneuver*>(IMC::Factory::produce(argv[5])));
  }
  else if (strcmp(argv[3], "ButtonEvent") == 0)
  {
    IMC::ButtonEvent* tmsg = new IMC::ButtonEvent;
    msg = tmsg;
    tmsg->button = atoi(argv[4]);
    tmsg->value = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "SetLedBrightness") == 0)
  {
    IMC::SetLedBrightness* tmsg = new IMC::SetLedBrightness;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->value = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "EstimatedState") == 0)
  {
    IMC::EstimatedState* tmsg = new IMC::EstimatedState;
    msg = tmsg;
    tmsg->x = atof(argv[4]);
    tmsg->y = atof(argv[5]);
    tmsg->z = atof(argv[6]);
    tmsg->phi = 0.0;
    tmsg->theta = 0.0;
    tmsg->psi = 0.0;
    tmsg->u = 0.0;
    tmsg->v = 0.0;
    tmsg->w = 0.0;
    tmsg->p = 0.0;
    tmsg->q = 0.0;
    tmsg->r = 0.0;
    tmsg->lat = 0.0;
    tmsg->lon = 0.0;
    tmsg->depth = 0.0;
  }
  else if (strcmp(argv[3], "PowerChannelControl") == 0)
  {
    IMC::PowerChannelControl* tmsg = new IMC::PowerChannelControl;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->op = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "AcousticSystemsQuery") == 0)
  {
    IMC::AcousticSystemsQuery* tmsg = new IMC::AcousticSystemsQuery;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "AcousticRange") == 0)
  {
    IMC::AcousticRange* tmsg = new IMC::AcousticRange;
    msg = tmsg;
    tmsg->address = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "AcousticMessage") == 0)
  {
    IMC::AcousticMessage* tmsg = new IMC::AcousticMessage;
    msg = tmsg;
    IMC::Message* imsg = IMC::Factory::produce(atoi(argv[4]));
    tmsg->message.set(*imsg);
    delete imsg;
  }
  else if (strcmp(argv[3], "AcousticPing") == 0)
  {
    msg = new IMC::AcousticPing;
  }
  else if (strcmp(argv[3], "QueryEntityInfo") == 0)
  {
    IMC::QueryEntityInfo* tmsg = new IMC::QueryEntityInfo;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "QueryEntityParameters") == 0)
  {
    IMC::QueryEntityParameters* tmsg = new IMC::QueryEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "SaveEntityParameters") == 0)
  {
    IMC::SaveEntityParameters* tmsg = new IMC::SaveEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "EntityList") == 0)
  {
    IMC::EntityList* tmsg = new IMC::EntityList;
    msg = tmsg;
    tmsg->op = IMC::EntityList::OP_QUERY;
  }
  else if (strcmp(argv[3], "ControlLoops") == 0)
  {
    IMC::ControlLoops* tmsg = new IMC::ControlLoops;
    msg = tmsg;
    tmsg->enable = atoi(argv[4]) ? 1 : 0;
    tmsg->mask = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "TeleoperationDone") == 0)
  {
    msg = new IMC::TeleoperationDone;
  }
  else if (strcmp(argv[3], "RemoteActionsRequest") == 0)
  {
    IMC::RemoteActionsRequest* tmsg = new IMC::RemoteActionsRequest;
    msg = tmsg;
    tmsg->op = IMC::RemoteActionsRequest::OP_QUERY;
  }
  else if (strcmp(argv[3], "RemoteActions") == 0)
  {
    IMC::RemoteActions* tmsg = new IMC::RemoteActions;
    msg = tmsg;
    tmsg->actions = argv[4];
  }
  else if (strcmp(argv[3], "LogBookControl") == 0)
  {
    IMC::LogBookControl* tmsg = new IMC::LogBookControl;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->htime = Time::Clock::getSinceEpoch() - atof(argv[5]);
    else
      tmsg->htime = -1;
    msg = tmsg;
  }
  else if (strcmp(argv[3], "EmergencyControl") == 0)
  {
    IMC::EmergencyControl* tmsg = new IMC::EmergencyControl;
    tmsg->command = atoi(argv[4]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "LeakSimulation") == 0)
  {
    IMC::LeakSimulation* tmsg = new IMC::LeakSimulation;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "OperationalLimits") == 0)
  {
    IMC::OperationalLimits* tmsg = new IMC::OperationalLimits;
    tmsg->mask = IMC::OPL_AREA;
    tmsg->lat = DUNE::Math::Angles::radians(atof(argv[4]));
    tmsg->lon = DUNE::Math::Angles::radians(atof(argv[5]));
    tmsg->orientation = DUNE::Math::Angles::radians(atof(argv[6]));
    tmsg->width = atof(argv[7]);
    tmsg->length = atof(argv[8]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "UASimulation") == 0)
  {
    IMC::UASimulation* tmsg = new IMC::UASimulation;
    tmsg->setSource(atoi(argv[4]));
    tmsg->setDestination(atoi(argv[5]));
    tmsg->speed = atoi(argv[6]);
    tmsg->type = IMC::UASimulation::UAS_DATA;
    tmsg->data.assign(atoi(argv[7]), '0');
    msg = tmsg;
  }
  else if (strcmp(argv[3], "ReplayControl") == 0)
  {
    IMC::ReplayControl* tmsg = new IMC::ReplayControl;
    tmsg->op = atoi(argv[4]);
    if (tmsg->op == IMC::ReplayControl::ROP_START)
      tmsg->file = argv[5];
    msg = tmsg;
  }
  else if (strcmp(argv[3], "ClockControl") == 0)
  {
    IMC::ClockControl* tmsg = new IMC::ClockControl;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->clock = atof(argv[5]);
    if (argc >= 7)
      tmsg->tz = atoi(argv[6]);
    msg = tmsg;
  }
  else if (strcmp(argv[3], "PlanControl") == 0)
  {
    IMC::PlanControl* tmsg = new IMC::PlanControl;
    tmsg->type = IMC::PlanControl::PC_REQUEST;
    tmsg->op = atoi(argv[4]);
    tmsg->plan_id = argv[5];
    if (argc >= 7)
      tmsg->flags = atoi(argv[6]);

    if (argc >= 8)
      tmsg->arg.set(IMC::Factory::produce(argv[7]));

    msg = tmsg;
  }
  else if (strcmp(argv[3], "LogBookEntry") == 0)
  {
    IMC::LogBookEntry* tmsg = new IMC::LogBookEntry;
    msg = tmsg;
    tmsg->context = argv[4];
    tmsg->text = argv[5];
    tmsg->htime = Time::Clock::getSinceEpoch();
    if (argc > 6)
      tmsg->type = atoi(argv[6]);
    else
      tmsg->type = IMC::LogBookEntry::LBET_WARNING;
  }
  else if (strcmp(argv[3], "TrexCommand") == 0)
  {
    IMC::TrexCommand* tmsg = new IMC::TrexCommand;
    msg = tmsg;
    if (strcmp(argv[4], "DISABLE") == 0 || strcmp(argv[4], "1") == 0 )
        tmsg->command = 1;
    else if (strcmp(argv[4], "ENABLE") == 0 || strcmp(argv[4], "2") == 0 )
        tmsg->command = 2;
  }
  else if (strcmp(argv[3], "PlanGeneration") == 0)
  {
    IMC::PlanGeneration* tmsg = new IMC::PlanGeneration;
    msg = tmsg;
    tmsg->cmd = atoi(argv[4]);
    tmsg->op = atoi(argv[5]);
    tmsg->plan_id = argv[6];

    if (argc >= 8)
      tmsg->params = argv[7];
  }
  else if (strcmp(argv[3], "SoundSpeed") == 0)
  {
    IMC::SoundSpeed* tmsg = new IMC::SoundSpeed;
    msg = tmsg;
    tmsg->value = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "Parameter") == 0)
  {
    IMC::Parameter* tmsg = new IMC::Parameter;
    msg = tmsg;
    tmsg->section = argv[4];
    tmsg->param = argv[5];
    tmsg->value = argv[6];
  }
  else if (strcmp(argv[3], "DevCalibrationControl") == 0)
  {
    IMC::DevCalibrationControl * tmsg = new IMC::DevCalibrationControl;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->op = atoi(argv[5]);
  }
  else if (strcmp(argv[3], "RegisterManeuver") == 0)
  {
    IMC::RegisterManeuver* tmsg = new IMC::RegisterManeuver;
    msg = tmsg;
    tmsg->mid = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "Brake") == 0)
  {
    IMC::Brake* tmsg = new IMC::Brake;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }
  else if (strcmp(argv[3], "SetEntityParameters") == 0)
  {
    IMC::SetEntityParameters* tmsg = new IMC::SetEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];

    IMC::EntityParameter param;

    unsigned i = 4;

    while (1)
    {
      if (argc >= (int)i + 2)
      {
        ++i;
        param.name = argv[i];
        ++i;
        param.value = argv[i];

        tmsg->params.push_back(param);
      }
      else
      {
        break;
      }
    }
  }
  else if (strcmp(argv[3], "PushEntityParameters") == 0)
  {
    IMC::PushEntityParameters* tmsg = new IMC::PushEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "PopEntityParameters") == 0)
  {
    IMC::PopEntityParameters* tmsg = new IMC::PopEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }
  else if (strcmp(argv[3], "IridiumMsgTx") == 0)
  {
    IMC::IridiumMsgTx* tmsg = new IMC::IridiumMsgTx;
    msg = tmsg;
    tmsg->req_id = atoi(argv[4]);
    tmsg->ttl = atoi(argv[5]);
    std::string hex = String::fromHex(argv[6]);
    tmsg->data.assign(hex.begin(), hex.end());
  }

  if (msg == NULL)
  {
    fprintf(stderr, "ERROR: unknown message '%s'\n", argv[3]);
    return 1;
  }

  msg->setTimeStamp();

  uint8_t bfr[1024] = {0};
  uint16_t rv = IMC::Packet::serialize(msg, bfr, sizeof(bfr));

  UDPSocket sock;
  try
  {
    sock.write((const char*)bfr, rv, dest, port);

    fprintf(stderr, "Raw:");
    for (int i = 0; i < rv; ++i)
      fprintf(stderr, " %02X", bfr[i]);
    fprintf(stderr, "\n");

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

  if (msg != NULL)
  {
    delete msg;
    msg = NULL;
  }

  return 0;
}
Example #9
0
    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);
    }
Example #10
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 #11
0
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;
            }
          }

          if (rdata.gotAll())
          {
            rdata.writeToStream(csv);
            rdata.clearData();
          }
        }

        delete msg;

        // ignore idles
        // either has the string _idle or has only 
        if (got_name &&
            (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;
    }

    csv.flush();

    delete is;

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

  csv.close();

  return 0;
}
Example #12
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;
    }
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
  }

  lsf.close();

  delete is;

  return 0;
}
Example #13
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 #14
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;
}
Example #15
0
    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;

        std::pair<std::string, Profile > p_pair((*itr)->maneuver_id, prof);
        m_profiles.insert(p_pair);
      }

      Memory::clear(m_accum_dur);
      Memory::clear(m_speed_vec);

      m_last_valid = nodes.back()->maneuver_id;
      m_finite_duration = true;
      return;
    }
Example #16
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 #17
0
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;
}
Example #18
0
    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);
    }
Example #19
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 #20
0
int
main(int argc, char** argv)
{
  if (argc < 4)
  {
    fprintf(stderr, "DUNE message sender\n\n");

    if (argc == 2 && (!strcmp(argv[1], "-l") || !strcmp(argv[1], "--list")))
    {
      fprintf(stdout, "Available Messages:\n");
      fprintf(stdout, "  [A]: Abort, AcousticMessage, AcousticOperation, AcousticSystemsQuery\n");
      fprintf(stdout, "  [B]: Brake, ButtonEvent\n");
      fprintf(stdout, "  [C]: CacheControl, Calibration, ClockControl, ControlLoops\n");
      fprintf(stdout, "  [D]: DataSanity, DesiredControl, DesiredHeading, DesiredHeadingRate, DesiredPitch,\n");
      fprintf(stdout, "       DesiredSpeed, DesiredRoll, DesiredZ, DevCalibrationControl, DevDataText\n");
      fprintf(stdout, "  [E]: EmergencyControl, EntityList, EntityState, EntityActivationState, EstimatedState\n");
      fprintf(stdout, "  [F]: FuelLevel\n");
      fprintf(stdout, "  [G]: GpsFix, GpsFixRtk\n");
      fprintf(stdout, "  [H]: Heartbeat\n");
      fprintf(stdout, "  [I]: IridiumMsgTx\n");
      fprintf(stdout, "  [L]: LblConfig, LblRange, LeaderState, LeakSimulation, LogBookControl, LogBookEntry,\n");
      fprintf(stdout, "       LoggingControl\n");
      fprintf(stdout, "  [M]: MagneticField, MonitorEntityState\n");
      fprintf(stdout, "  [O]: OperationalLimits\n");
      fprintf(stdout, "  [P]: PlanControl, PlanGeneration, PopEntityParameters, PowerChannelControl,\n");
      fprintf(stdout, "       PowerChannelState, PushEntityParameters\n");
      fprintf(stdout, "  [Q]: QueryEntityInfo, QueryEntityParameters\n");
      fprintf(stdout, "  [R]: RegisterManeuver, RemoteActions, RemoteActionsRequest, ReplayControl, ReportControl,\n");
      fprintf(stdout, "       RestartSystem\n");
      fprintf(stdout, "  [S]: SaveEntityParameters, SetEntityParameters, SetLedBrightness, SetServoPosition,\n");
      fprintf(stdout, "       SetThrusterActuation, Sms, SoundSpeed\n");
      fprintf(stdout, "  [T]: Target, TeleoperationDone, Temperature, TextMessage, TrexCommand\n");
      fprintf(stdout, "  [U]: UASimulation\n");
      fprintf(stdout, "  [V]: VehicleCommand, VehicleMedium\n");
      return 1;
    }

    fprintf(stderr, "Usage:\n");
    fprintf(stderr, "  %s <destination host> <destination port> <abbrev> [arguments]\n\n", argv[0]);
    fprintf(stderr, "Options:\n");
    fprintf(stderr, "  -l, --list                            Print list of acceptable messages\n\n\n");
    return 1;
  }

  Address dest(argv[1]);

  // Parse port.
  unsigned port = 0;
  if (!castLexical(argv[2], port))
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  if (port > 65535)
  {
    fprintf(stderr, "ERROR: invalid port '%s'\n", argv[2]);
    return 1;
  }

  IMC::Message* msg = NULL;

  if (strcmp(argv[3], "Abort") == 0)
  {
    msg = new IMC::Abort;

    if (argc == 5)
      msg->setDestination(atoi(argv[4]));
  }

  if (strcmp(argv[3], "AcousticMessage") == 0)
  {
    IMC::AcousticMessage* tmsg = new IMC::AcousticMessage;
    msg = tmsg;
    IMC::Message* imsg = IMC::Factory::produce(atoi(argv[4]));
    tmsg->message.set(*imsg);
    delete imsg;
  }

  if (strcmp(argv[3], "AcousticOperation") == 0)
  {
    IMC::AcousticOperation* tmsg = new IMC::AcousticOperation;
    msg = tmsg;
    tmsg->op = IMC::AcousticOperation::AOP_RANGE_RECVED;
    tmsg->system = argv[4];
    tmsg->range = atoi(argv[5]);
  }

  if (strcmp(argv[3], "AcousticSystemsQuery") == 0)
  {
    IMC::AcousticSystemsQuery* tmsg = new IMC::AcousticSystemsQuery;
    msg = tmsg;
  }

  if (strcmp(argv[3], "Brake") == 0)
  {
    IMC::Brake* tmsg = new IMC::Brake;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }

  if (strcmp(argv[3], "ButtonEvent") == 0)
  {
    IMC::ButtonEvent* tmsg = new IMC::ButtonEvent;
    msg = tmsg;
    tmsg->button = atoi(argv[4]);
    tmsg->value = atoi(argv[5]);
  }

  if (strcmp(argv[3], "CacheControl") == 0)
  {
    IMC::CacheControl* tmsg = new IMC::CacheControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
  }

  if (strcmp(argv[3], "Calibration") == 0)
  {
    IMC::Calibration* tmsg = new IMC::Calibration;
    tmsg->duration = (uint16_t)(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "ClockControl") == 0)
  {
    IMC::ClockControl* tmsg = new IMC::ClockControl;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->clock = atof(argv[5]);
    if (argc >= 7)
      tmsg->tz = atoi(argv[6]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "ControlLoops") == 0)
  {
    IMC::ControlLoops* tmsg = new IMC::ControlLoops;
    msg = tmsg;
    tmsg->enable = atoi(argv[4]) ? 1 : 0;
    tmsg->mask = atoi(argv[5]);
    tmsg->scope_ref = atoi(argv[6]);
  }

  if (strcmp(argv[3], "DataSanity") == 0)
  {
    IMC::DataSanity* tmsg = new IMC::DataSanity;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->sane = atoi(argv[5]);
  }

  if (strcmp(argv[3], "DesiredControl") == 0)
  {
    IMC::DesiredControl* tmsg = new IMC::DesiredControl;
    tmsg->k = atof(argv[4]);
    tmsg->m = atof(argv[5]);
    tmsg->n = atof(argv[6]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredHeading") == 0)
  {
    IMC::DesiredHeading* tmsg = new IMC::DesiredHeading;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredHeadingRate") == 0)
  {
    IMC::DesiredHeadingRate* tmsg = new IMC::DesiredHeadingRate;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredPitch") == 0)
  {
    IMC::DesiredPitch* tmsg = new IMC::DesiredPitch;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredSpeed") == 0)
  {
    IMC::DesiredSpeed* tmsg = new IMC::DesiredSpeed;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->speed_units = IMC::SUNITS_PERCENTAGE;
    else
      tmsg->speed_units = atoi(argv[5]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredRoll") == 0)
  {
    IMC::DesiredRoll* tmsg = new IMC::DesiredRoll;
    tmsg->value = DUNE::Math::Angles::radians(atof(argv[4]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DesiredZ") == 0)
  {
    IMC::DesiredZ* tmsg = new IMC::DesiredZ;
    tmsg->value = atof(argv[4]);
    if (argc == 5)
      tmsg->z_units = static_cast<IMC::ZUnits>(3);
    else
      tmsg->z_units = static_cast<IMC::ZUnits>(atoi(argv[5]));
    msg = tmsg;
  }

  if (strcmp(argv[3], "DevCalibrationControl") == 0)
  {
    IMC::DevCalibrationControl * tmsg = new IMC::DevCalibrationControl;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->op = atoi(argv[5]);
  }

  if (strcmp(argv[3], "DevDataText") == 0)
  {
    IMC::DevDataText * tmsg = new IMC::DevDataText;
    msg = tmsg;
    tmsg->value = argv[4];
  }

  if (strcmp(argv[3], "EmergencyControl") == 0)
  {
    IMC::EmergencyControl* tmsg = new IMC::EmergencyControl;
    tmsg->command = atoi(argv[4]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "EntityList") == 0)
  {
    IMC::EntityList* tmsg = new IMC::EntityList;
    msg = tmsg;
    tmsg->op = IMC::EntityList::OP_QUERY;
  }

  if (strcmp(argv[3], "EntityState") == 0)
  {
    IMC::EntityState* tmsg = new IMC::EntityState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = atoi(argv[5]);
  }

  if (strcmp(argv[3], "EntityActivationState") == 0)
  {
    IMC::EntityActivationState* tmsg = new IMC::EntityActivationState;
    msg = tmsg;
    tmsg->setSourceEntity(atoi(argv[4]));
    tmsg->state = IMC::EntityActivationState::EAS_ACTIVE;
  }

  if (strcmp(argv[3], "EstimatedState") == 0)
  {
    IMC::EstimatedState* tmsg = new IMC::EstimatedState;
    msg = tmsg;
    tmsg->x = atof(argv[4]);
    tmsg->y = atof(argv[5]);
    tmsg->z = atof(argv[6]);
    tmsg->phi = 0.0;
    tmsg->theta = 0.0;
    tmsg->psi = 0.0;
    tmsg->u = 0.0;
    tmsg->v = 0.0;
    tmsg->w = 0.0;
    tmsg->p = 0.0;
    tmsg->q = 0.0;
    tmsg->r = 0.0;
    tmsg->lat = 0.0;
    tmsg->lon = 0.0;
    tmsg->depth = 0.0;
  }

  if (strcmp(argv[3], "FuelLevel") == 0)
  {
    IMC::FuelLevel* tmsg = new IMC::FuelLevel;
    msg = tmsg;
    tmsg->setSource(atof(argv[4]));
    tmsg->value = atof(argv[5]);
    tmsg->confidence = atof(argv[6]);
  }

  if (strcmp(argv[3], "GpsFix") == 0)
  {
    IMC::GpsFix* tmsg = new IMC::GpsFix;
    msg = tmsg;
    tmsg->type = IMC::GpsFix::GFT_DIFFERENTIAL;
    tmsg->satellites = 10;
    tmsg->validity = 0xFFFF;

    if (argc >= 5)
    {
      tmsg->lat = Angles::radians(atof(argv[4]));
      tmsg->lon = Angles::radians(atof(argv[5]));
    }
    else
    {
      // Leixões harbor location.
      tmsg->lat = 0.718803520085;
      tmsg->lon = -0.151951035032;
    }

    if (argc >= 7)
      tmsg->height = atof(argv[6]);
  }

  if (strcmp(argv[3], "GpsFixRtk") == 0)
  {
    IMC::GpsFixRtk* tmsg = new IMC::GpsFixRtk;
    msg = tmsg;
    tmsg->type = IMC::GpsFixRtk::RTK_FIXED;
    tmsg->satellites = 10;
    tmsg->iar_hyp = 1;
    tmsg->setSource(0x2c01);

    if (argc >= 5)
      tmsg->setSource(tmsg->getSource() + atoi(argv[4]));

    if (argc >= 6)
    {
      if (!strcmp(argv[5], "Float"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_FLOAT;
      }
      else if (!strcmp(argv[5], "Obs"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_OBS;
      }
      else if (!strcmp(argv[5], "None"))
      {
        tmsg->type = IMC::GpsFixRtk::RTK_NONE;
      }
      else
      {
        tmsg->type = IMC::GpsFixRtk::RTK_FIXED;
      }
    }
    if (argc >= 9)
    {
      tmsg->n = atof(argv[6]);
      tmsg->e = atof(argv[7]);
      tmsg->d = atof(argv[8]);

    }
    else
    {
      // Default location
      tmsg->n = 4.0;
      tmsg->e = 3.0;
      tmsg->d = -2.0;
    }

    if (argc == 7)
    {
      tmsg->iar_hyp = atoi(argv[6]);
    }
  }

  if (strcmp(argv[3], "Heartbeat") == 0)
  {
    IMC::Heartbeat* tmsg = new IMC::Heartbeat;

    if (argc > 4)
      tmsg->setSource(atoi(argv[4]));

    if (argc > 5)
      tmsg->setDestination(atoi(argv[5]));

    msg = tmsg;
  }

  if (strcmp(argv[3], "IridiumMsgTx") == 0)
  {
    IMC::IridiumMsgTx* tmsg = new IMC::IridiumMsgTx;
    msg = tmsg;
    tmsg->req_id = atoi(argv[4]);
    tmsg->ttl = atoi(argv[5]);
    std::string hex = String::fromHex(argv[6]);
    tmsg->data.assign(hex.begin(), hex.end());
  }

  if (strcmp(argv[3], "LblConfig") == 0)
  {
    IMC::LblConfig* tmsg = new IMC::LblConfig;
    msg = tmsg;
    tmsg->op = IMC::LblConfig::OP_SET_CFG;

    IMC::LblBeacon bc;
    bc.beacon = "benthos2";
    bc.lat = 0.71883274;
    bc.lon = -0.15194732;
    bc.depth = 3;
    tmsg->beacons.push_back(bc);
    bc.beacon = "benthos3";
    bc.lat = 0.71881068;
    bc.lon = -0.15192335;
    tmsg->beacons.push_back(bc);
  }

  if (strcmp(argv[3], "LblRange") == 0)
  {
    IMC::LblRange* tmsg = new IMC::LblRange;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->range = atoi(argv[5]);
  }

  if (strcmp(argv[3], "LeaderState") == 0)
  {
    IMC::LeaderState* tmsg = new IMC::LeaderState;
    msg = tmsg;
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->height = atof(argv[6]);
  }

  if (strcmp(argv[3], "LeakSimulation") == 0)
  {
    IMC::LeakSimulation* tmsg = new IMC::LeakSimulation;
    tmsg->op = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
    msg = tmsg;
  }

  if (strcmp(argv[3], "LogBookControl") == 0)
  {
    IMC::LogBookControl* tmsg = new IMC::LogBookControl;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->htime = Time::Clock::getSinceEpoch() - atof(argv[5]);
    else
      tmsg->htime = -1;
    msg = tmsg;
  }

  if (strcmp(argv[3], "LogBookEntry") == 0)
  {
    IMC::LogBookEntry* tmsg = new IMC::LogBookEntry;
    msg = tmsg;
    tmsg->context = argv[4];
    tmsg->text = argv[5];
    tmsg->htime = Time::Clock::getSinceEpoch();
    if (argc > 6)
      tmsg->type = atoi(argv[6]);
    else
      tmsg->type = IMC::LogBookEntry::LBET_WARNING;
  }

  if (strcmp(argv[3], "LoggingControl") == 0)
  {
    IMC::LoggingControl* tmsg = new IMC::LoggingControl;
    msg = tmsg;
    tmsg->op = atoi(argv[4]);
    tmsg->name = argv[5];
  }

  if (strcmp(argv[3], "MagneticField") == 0)
  {
    IMC::MagneticField* tmsg = new IMC::MagneticField;
    msg = tmsg;
    tmsg->setDestinationEntity(atoi(argv[4]));
    tmsg->x = atof(argv[5]);
    tmsg->y = atof(argv[6]);
    tmsg->z = atof(argv[7]);
  }

  if (strcmp(argv[3], "MonitorEntityState") == 0)
  {
    IMC::MonitorEntityState* tmsg = new IMC::MonitorEntityState;
    msg = tmsg;
    tmsg->command = atoi(argv[4]);
    if (argc >= 6)
      tmsg->entities = argv[5];
  }

  if (strcmp(argv[3], "OperationalLimits") == 0)
  {
    IMC::OperationalLimits* tmsg = new IMC::OperationalLimits;
    tmsg->mask = IMC::OPL_AREA;
    tmsg->lat = DUNE::Math::Angles::radians(atof(argv[4]));
    tmsg->lon = DUNE::Math::Angles::radians(atof(argv[5]));
    tmsg->orientation = DUNE::Math::Angles::radians(atof(argv[6]));
    tmsg->width = atof(argv[7]);
    tmsg->length = atof(argv[8]);
    msg = tmsg;
  }

  if (strcmp(argv[3], "PlanControl") == 0)
  {
    IMC::PlanControl* tmsg = new IMC::PlanControl;
    tmsg->type = IMC::PlanControl::PC_REQUEST;
    tmsg->op = atoi(argv[4]);
    tmsg->plan_id = argv[5];
    if (argc >= 7)
      tmsg->flags = atoi(argv[6]);

    if (argc >= 8)
      tmsg->arg.set(IMC::Factory::produce(argv[7]));

    msg = tmsg;
  }

  if (strcmp(argv[3], "PlanGeneration") == 0)
  {
    IMC::PlanGeneration* tmsg = new IMC::PlanGeneration;
    msg = tmsg;
    tmsg->cmd = atoi(argv[4]);
    tmsg->op = atoi(argv[5]);
    tmsg->plan_id = argv[6];

    if (argc >= 8)
      tmsg->params = argv[7];
  }

  if (strcmp(argv[3], "PopEntityParameters") == 0)
  {
    IMC::PopEntityParameters* tmsg = new IMC::PopEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "PowerChannelControl") == 0)
  {
    IMC::PowerChannelControl* tmsg = new IMC::PowerChannelControl;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->op = atoi(argv[5]);
  }

  if (strcmp(argv[3], "PowerChannelState") == 0)
  {
    IMC::PowerChannelState* tmsg = new IMC::PowerChannelState;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->state = atoi(argv[5]);
  }

  if (strcmp(argv[3], "PushEntityParameters") == 0)
  {
    IMC::PushEntityParameters* tmsg = new IMC::PushEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "QueryEntityInfo") == 0)
  {
    IMC::QueryEntityInfo* tmsg = new IMC::QueryEntityInfo;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
  }

  if (strcmp(argv[3], "QueryEntityParameters") == 0)
  {
    IMC::QueryEntityParameters* tmsg = new IMC::QueryEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "RegisterManeuver") == 0)
  {
    IMC::RegisterManeuver* tmsg = new IMC::RegisterManeuver;
    msg = tmsg;
    tmsg->mid = atoi(argv[4]);
  }

  if (strcmp(argv[3], "RemoteActions") == 0)
  {
    IMC::RemoteActions* tmsg = new IMC::RemoteActions;
    msg = tmsg;
    tmsg->actions = argv[4];
  }

  if (strcmp(argv[3], "RemoteActionsRequest") == 0)
  {
    IMC::RemoteActionsRequest* tmsg = new IMC::RemoteActionsRequest;
    msg = tmsg;
    tmsg->op = IMC::RemoteActionsRequest::OP_QUERY;
  }

  if (strcmp(argv[3], "ReplayControl") == 0)
  {
    IMC::ReplayControl* tmsg = new IMC::ReplayControl;
    tmsg->op = atoi(argv[4]);
    if (tmsg->op == IMC::ReplayControl::ROP_START)
      tmsg->file = argv[5];
    msg = tmsg;
  }

  if (strcmp(argv[3], "ReportControl") == 0)
  {
    IMC::ReportControl* tmsg = new IMC::ReportControl;
    tmsg->op = atoi(argv[4]);
    tmsg->comm_interface = atoi(argv[5]);
    tmsg->period = atoi(argv[6]);
    tmsg->sys_dst = argv[7];
    msg = tmsg;
  }

  if (strcmp(argv[3], "RestartSystem") == 0)
  {
    IMC::RestartSystem* tmsg = new IMC::RestartSystem;
    msg = tmsg;
  }

  if (strcmp(argv[3], "SaveEntityParameters") == 0)
  {
    IMC::SaveEntityParameters* tmsg = new IMC::SaveEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];
  }

  if (strcmp(argv[3], "SetEntityParameters") == 0)
  {
    IMC::SetEntityParameters* tmsg = new IMC::SetEntityParameters;
    msg = tmsg;
    tmsg->name = argv[4];

    IMC::EntityParameter param;

    unsigned i = 4;

    while (1)
    {
      if (argc >= (int)i + 2)
      {
        ++i;
        param.name = argv[i];
        ++i;
        param.value = argv[i];

        tmsg->params.push_back(param);
      }
      else
      {
        break;
      }
    }
  }

  if (strcmp(argv[3], "SetLedBrightness") == 0)
  {
    IMC::SetLedBrightness* tmsg = new IMC::SetLedBrightness;
    msg = tmsg;
    tmsg->name = argv[4];
    tmsg->value = atoi(argv[5]);
  }

  if (strcmp(argv[3], "SetServoPosition") == 0)
  {
    IMC::SetServoPosition* tmsg = new IMC::SetServoPosition;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }

  if (strcmp(argv[3], "SetThrusterActuation") == 0)
  {
    IMC::SetThrusterActuation* tmsg = new IMC::SetThrusterActuation;
    msg = tmsg;
    tmsg->id = atoi(argv[4]);
    tmsg->value = atof(argv[5]);
  }

  if (strcmp(argv[3], "Sms") == 0)
  {
    IMC::Sms* tmsg = new IMC::Sms;
    tmsg->number = argv[4];
    tmsg->timeout = atoi(argv[5]);
    tmsg->contents = argv[6];
    msg = tmsg;
  }

  if (strcmp(argv[3], "SoundSpeed") == 0)
  {
    IMC::SoundSpeed* tmsg = new IMC::SoundSpeed;
    msg = tmsg;
    tmsg->value = atoi(argv[4]);
  }

  if (strcmp(argv[3], "Target") == 0)
  {
    IMC::Target* tmsg = new IMC::Target;
    msg = tmsg;
    tmsg->label = "dune-sendmsg";
    tmsg->lat = Angles::radians(atof(argv[4]));
    tmsg->lon = Angles::radians(atof(argv[5]));
    tmsg->z = atof(argv[6]);

    if (argc > 7)
    {
      if (!strcmp(argv[7], "DEP") || !strcmp(argv[7], "dep"))
        tmsg->z_units = IMC::Z_DEPTH;
      else if (!strcmp(argv[7], "ALT") || !strcmp(argv[7], "alt"))
        tmsg->z_units = IMC::Z_ALTITUDE;
      else if (!strcmp(argv[7], "HEI") || !strcmp(argv[7], "hei"))
        tmsg->z_units = IMC::Z_HEIGHT;
    }

    if (argc > 9)
    {
      tmsg->cog = Angles::normalizeRadian(Angles::radians(atof(argv[8])));
      tmsg->sog = atof(argv[9]);
    }
  }

  if (strcmp(argv[3], "Temperature") == 0)
  {
    IMC::Temperature* tmsg = new IMC::Temperature;
    msg = tmsg;
    tmsg->value = atof(argv[4]);
  }

  if (strcmp(argv[3], "TeleoperationDone") == 0)
  {
    msg = new IMC::TeleoperationDone;
  }

  if (strcmp(argv[3], "TextMessage") == 0)
  {
    IMC::TextMessage* tmsg = new IMC::TextMessage;
    msg = tmsg;
    if (argc >= 6) {
      tmsg->origin = argv[4];
      tmsg->text = argv[5];
    }
    else if (argc == 5)
    {
      tmsg->origin = "dune-sendmsg";
      tmsg->text = argv[4];
    }
  }

  if (strcmp(argv[3], "TrexCommand") == 0)
  {
    IMC::TrexCommand* tmsg = new IMC::TrexCommand;
    msg = tmsg;
    if (strcmp(argv[4], "DISABLE") == 0 || strcmp(argv[4], "1") == 0 )
        tmsg->command = 1;
    else if (strcmp(argv[4], "ENABLE") == 0 || strcmp(argv[4], "2") == 0 )
        tmsg->command = 2;
  }

  if (strcmp(argv[3], "UASimulation") == 0)
  {
    IMC::UASimulation* tmsg = new IMC::UASimulation;
    tmsg->setSource(atoi(argv[4]));
    tmsg->setDestination(atoi(argv[5]));
    tmsg->speed = atoi(argv[6]);
    tmsg->type = IMC::UASimulation::UAS_DATA;
    tmsg->data.assign(atoi(argv[7]), '0');
    msg = tmsg;
  }

  if (strcmp(argv[3], "UsblConfig") == 0)
  {
    IMC::UsblConfig* tmsg = new IMC::UsblConfig;
    msg = tmsg;
    tmsg->op = IMC::UsblConfig::OP_SET_CFG;

    IMC::UsblModem modem;
    modem.name = argv[4];
    modem.lat = atof(argv[5]);
    modem.lon = atof(argv[6]);
    modem.z = atof(argv[7]);
    modem.z_units = static_cast<IMC::ZUnits>(1);
    tmsg->modems.push_back(modem);
  }

  if (strcmp(argv[3], "VehicleCommand") == 0)
  {
    IMC::VehicleCommand* tmsg = new IMC::VehicleCommand;
    msg = tmsg;
    tmsg->type = IMC::VehicleCommand::VC_REQUEST;
    tmsg->command = atoi(argv[4]);

    if (tmsg->command == IMC::VehicleCommand::VC_EXEC_MANEUVER)
      tmsg->maneuver.set(static_cast<IMC::Maneuver*>(IMC::Factory::produce(argv[5])));
  }

  if (strcmp(argv[3], "VehicleMedium") == 0)
  {
    IMC::VehicleMedium* tmsg = new IMC::VehicleMedium;
    msg = tmsg;
    tmsg->medium = atoi(argv[4]);
  }

  if (msg == NULL)
  {
    fprintf(stderr, "ERROR: unknown message '%s'\n", argv[3]);
    return 1;
  }

  msg->setTimeStamp();

  uint8_t bfr[1024] = {0};
  uint16_t rv = IMC::Packet::serialize(msg, bfr, sizeof(bfr));

  UDPSocket sock;
  try
  {
    sock.write(bfr, rv, dest, port);

    fprintf(stderr, "Raw:");
    for (int i = 0; i < rv; ++i)
      fprintf(stderr, " %02X", bfr[i]);
    fprintf(stderr, "\n");

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

  if (msg != NULL)
  {
    delete msg;
    msg = NULL;
  }

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