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