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