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