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