Task(const std::string& name, Tasks::Context& ctx):
DUNE::Tasks::Task(name, ctx)
{
// Define configuration parameters.
param("Entity Label - GPS", m_args.elabel_gps)
.description("Entity label of 'GpsFix' and 'GroundVelocity' messages");
param("Entity Label - IMU", m_args.elabel_imu)
.description("Entity label of 'EulerAngles' and 'AngularVelocity' messages");
param("Entity Label - Yaw", m_args.elabel_yaw)
.description("Entity label of 'EulerAngles' messages (field 'psi')");
m_estate.clear();
// Navigation enters error mode without valid GPS data.
m_time_without_gps.setTop(5.0);
// Register callbacks
bind<IMC::AngularVelocity>(this);
bind<IMC::EulerAngles>(this);
bind<IMC::GpsFix>(this);
bind<IMC::GroundVelocity>(this);
}
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;
}
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;
}
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;
}
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;
}
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;
}
int
main(int32_t argc, char** argv)
{
if (argc != 3)
{
std::cerr << "Usage: " << argv[0] << " <config ini file> <path_to_log.lsf[.gz]>"
<< std::endl;
return 1;
}
Arguments m_args;
readArgs(argv[1], m_args);
m_args.filter_args.decay_factor *= 0.01f;
//! Array of entities
unsigned m_eids[BatteryData::BM_TOTAL];
// Filter pointer
FuelFilter* m_fuel_filter = NULL;
bool resolved_entities[BatteryData::BM_TOTAL];
for (unsigned i = 0; i < BatteryData::BM_TOTAL; ++i)
resolved_entities[i] = false;
EPMap m_epower;
bool got_entities = false;
PseudoTimer timer;
PeriodicRun pr(1.0);
ByteBuffer buffer;
std::ofstream lsf("NewFuel.lsf", std::ios::binary);
std::istream* is = 0;
DUNE::Compression::Methods method = DUNE::Compression::Factory::detect(argv[2]);
if (method == DUNE::Compression::METHOD_UNKNOWN)
is = new std::ifstream(argv[2], std::ios::binary);
else
is = new DUNE::Compression::FileInput(argv[2], method);
if (!is)
{
std::cerr << "bad file" << std::endl;
return 1;
}
is->seekg(0, is->end);
unsigned file_length = is->tellg();
is->seekg(0, is->beg);
Time::Counter<float> prog_timer(5.0);
DUNE::IMC::Message* msg = NULL;
DUNE::IMC::FuelLevel* ptr = NULL;
bool got_first = false;
try
{
while ((msg = DUNE::IMC::Packet::deserialize(*is)) != 0)
{
bool log_it = false;
timer.update(msg->getTimeStamp());
if (!got_first)
{
IMC::EstimatedState state;
state.setTimeStamp(msg->getTimeStamp());
IMC::Packet::serialize(&state, buffer);
lsf.write(buffer.getBufferSigned(), buffer.getSize());
got_first = true;
std::cerr << "got first timestamp" << std::endl;
m_fuel_filter = new FuelFilter(&m_args.filter_args, m_eids, &m_epower,
NULL, true, msg->getTimeStamp());
}
if (msg->getId() == DUNE_IMC_ENTITYINFO)
{
IMC::EntityInfo* ent = static_cast<IMC::EntityInfo*>(msg);
if (!got_entities)
{
bool got_all = true;
for (unsigned i = 0; i < BatteryData::BM_TOTAL; ++i)
{
if (ent->label == m_args.elb[i])
{
m_eids[i] = ent->id;
resolved_entities[i] = true;
}
if (resolved_entities[i] == false)
got_all = false;
}
got_entities = got_all;
if (got_entities)
std::cerr << "Got all entities" << std::endl;
}
for (unsigned i = 0; i < m_args.est_list.size(); i++)
{
if (ent->label == m_args.est_list[i])
m_epower.insert(EPPair(ent->id, EntityPower(m_args.est_power[i])));
}
if (m_args.est_list.size() < m_epower.size())
std::cerr << "TOO MANY ENTRIES!" << std::endl;
IMC::Packet::serialize(msg, buffer);
lsf.write(buffer.getBufferSigned(), buffer.getSize());
delete msg;
continue;
}
if (msg->getId() == DUNE_IMC_VOLTAGE)
{
m_fuel_filter->onVoltage(static_cast<IMC::Voltage*>(msg));
log_it = true;
}
else if (msg->getId() == DUNE_IMC_CURRENT)
{
m_fuel_filter->onCurrent(static_cast<IMC::Current*>(msg));
log_it = true;
}
else if (msg->getId() == DUNE_IMC_TEMPERATURE)
{
m_fuel_filter->onTemperature(static_cast<IMC::Temperature*>(msg));
log_it = true;
}
else if (msg->getId() == DUNE_IMC_VEHICLESTATE)
{
m_fuel_filter->onVehicleState(static_cast<IMC::VehicleState*>(msg));
log_it = true;
}
else if (msg->getId() == DUNE_IMC_FUELLEVEL)
{
log_it = true;
}
else if (msg->getId() == DUNE_IMC_ENTITYACTIVATIONSTATE)
{
m_fuel_filter->onEntityActivationState(static_cast<IMC::EntityActivationState*>(msg));
log_it = true;
}
if (timer.isValid() && pr.doRun(timer.getTime()))
{
// Update fuel filter
if (m_fuel_filter->update())
{
IMC::FuelLevel fl;
fl.setSourceEntity(250);
fl.setTimeStamp(msg->getTimeStamp());
m_fuel_filter->fillMessage(fl, m_args.op_labels, m_args.op_values);
if (ptr != NULL)
{
float diff = ptr->value - fl.value;
char sign = (diff > 0)? '-' : '+';
if (std::fabs(diff) > 1.0)
std::cerr << "jumped " << sign
<< std::fabs(diff) / 100 * m_args.filter_args.full_capacity
<< " (" << std::fabs(diff) << "%)" << std::endl;
}
Memory::clear(ptr);
ptr = static_cast<IMC::FuelLevel*>(fl.clone());
IMC::Packet::serialize(&fl, buffer);
lsf.write(buffer.getBufferSigned(), buffer.getSize());
}
}
if (log_it)
{
IMC::Packet::serialize(msg, buffer);
lsf.write(buffer.getBufferSigned(), buffer.getSize());
}
if (prog_timer.overflow())
{
std::cerr << (float)is->tellg() / file_length * 100.0 << "%" << std::endl;
prog_timer.reset();
}
delete msg;
}
}
catch (std::runtime_error& e)
{
std::cerr << "ERROR: " << e.what() << std::endl;
}
Memory::clear(m_fuel_filter);
Memory::clear(ptr);
delete is;
return 0;
}
