// ======================================================================
// See TOutputRawlogCreator declaration
// ======================================================================
TOutputRawlogCreator::TOutputRawlogCreator()
{
if (!arg_output_file.isSet())
throw runtime_error("This operation requires an output file. Use '-o file' or '--output file'.");
out_rawlog_filename = arg_output_file.getValue();
if (fileExists(out_rawlog_filename) && !arg_overwrite.getValue() )
throw runtime_error(string("*ABORTING*: Output file already exists: ") + out_rawlog_filename + string("\n. Select a different output path, remove the file or force overwrite with '-w' or '--overwrite'.") );
if (!out_rawlog.open(out_rawlog_filename))
throw runtime_error(string("*ABORTING*: Cannot open output file: ") + out_rawlog_filename );
}
// ======================================================================
// See TOutputRawlogCreator declaration
// ======================================================================
TOutputRawlogCreator::TOutputRawlogCreator()
{
if (!arg_output_file.isSet())
throw runtime_error(
"This operation requires an output file. Use '-o file' or "
"'--output file'.");
out_rawlog_filename = arg_output_file.getValue();
if (fileExists(out_rawlog_filename) && !arg_overwrite.getValue())
throw runtime_error(
string("*ABORTING*: Output file already exists: ") +
out_rawlog_filename +
string("\n. Select a different output path, remove the file or "
"force overwrite with '-w' or '--overwrite'."));
if (!out_rawlog_io.open(out_rawlog_filename))
throw runtime_error(
string("*ABORTING*: Cannot open output file: ") +
out_rawlog_filename);
out_rawlog = std::make_unique<
mrpt::serialization::CArchiveStreamBase<mrpt::io::CFileGZOutputStream>>(
out_rawlog_io);
}
void thread_grabbing(TThreadParam& p)
{
try
{
CFileGZOutputStream f_out_rawlog;
if (arg_out_rawlog.isSet())
{
if (!f_out_rawlog.open(arg_out_rawlog.getValue()))
THROW_EXCEPTION_FMT(
"Error creating output rawlog file: %s",
arg_out_rawlog.getValue().c_str());
}
auto arch = mrpt::serialization::archiveFrom(f_out_rawlog);
mrpt::hwdrivers::CVelodyneScanner velodyne;
if (arg_verbose.isSet()) velodyne.enableVerbose(true);
// Set params:
velodyne.setModelName(mrpt::typemeta::TEnumType<
mrpt::hwdrivers::CVelodyneScanner::model_t>::
name2value(arg_model.getValue()));
if (arg_ip_filter.isSet())
velodyne.setDeviceIP(
arg_ip_filter.getValue()); // Default: from any IP
if (arg_in_pcap.isSet())
velodyne.setPCAPInputFile(arg_in_pcap.getValue());
if (arg_out_pcap.isSet())
velodyne.setPCAPOutputFile(arg_out_pcap.getValue());
// If you have a calibration file, better than default values:
if (arg_calib_file.isSet())
{
mrpt::obs::VelodyneCalibration calib;
if (!calib.loadFromXMLFile(arg_calib_file.getValue()))
throw std::runtime_error(
"Aborting: error loading calibration file.");
velodyne.setCalibration(calib);
}
// Open:
cout << "Calling CVelodyneScanner::initialize()...";
velodyne.initialize();
cout << "OK\n";
cout << "Waiting for first data packets (Press CTRL+C to abort)...\n";
CTicTac tictac;
int nScans = 0;
bool hard_error = false;
while (!hard_error && !p.quit)
{
// Grab new observations from the camera:
CObservationVelodyneScan::Ptr
obs; // (initially empty) Smart pointers to observations
CObservationGPS::Ptr obs_gps;
hard_error = !velodyne.getNextObservation(obs, obs_gps);
// Save to log file:
if (f_out_rawlog.fileOpenCorrectly())
{
if (obs) arch << *obs;
if (obs_gps) arch << *obs_gps;
}
if (obs)
{
std::atomic_store(&p.new_obs, obs);
nScans++;
}
if (obs_gps) std::atomic_store(&p.new_obs_gps, obs_gps);
if (p.pushed_key != 0)
{
switch (p.pushed_key)
{
case 27:
p.quit = true;
break;
}
// Clear pushed key flag:
p.pushed_key = 0;
}
if (nScans > 5)
{
p.Hz = nScans / tictac.Tac();
nScans = 0;
tictac.Tic();
}
}
}
catch (const std::exception& e)
{
cout << "Exception in Velodyne thread: " << mrpt::exception_to_str(e)
<< endl;
p.quit = true;
}
}
void execGraphSlamEngine(mrpt::system::COutputLogger* logger)
{
// Instance for managing the available graphslam deciders optimizers
TUserOptionsChecker<GRAPH_T> options_checker;
options_checker.createDeciderOptimizerMappings();
options_checker.populateDeciderOptimizerProperties();
// fetch the command line options
// ////////////////////////////////////////////////////////////
// decide whether to display the help messages for the deciders/optimizers
{
bool list_registrars = false;
if (list_all_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("all");
list_registrars = true;
}
if (list_node_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("node");
list_registrars = true;
}
if (list_edge_registrars.getValue())
{
options_checker.dumpRegistrarsToConsole("edge");
list_registrars = true;
}
if (list_optimizers.getValue())
{
options_checker.dumpOptimizersToConsole();
}
if (list_registrars || list_optimizers.getValue())
{
logger->logFmt(LVL_INFO, "Exiting.. ");
return;
}
}
// fetch the filenames
// ini file
string ini_fname = arg_ini_file.getValue();
// rawlog file
string rawlog_fname = arg_rawlog_file.getValue();
// ground-truth file
string ground_truth_fname;
if (arg_ground_truth_file.isSet())
{
ground_truth_fname = arg_ground_truth_file.getValue();
}
if (disable_visuals.getValue())
{ // enabling Visualization objects
logger->logFmt(LVL_WARN, "Running on headless mode - Visuals disabled");
}
// fetch which registration deciders / optimizer to use
string node_reg = arg_node_reg.getValue();
string edge_reg = arg_edge_reg.getValue();
string optimizer = arg_optimizer.getValue();
logger->logFmt(LVL_INFO, "Node registration decider: %s", node_reg.c_str());
logger->logFmt(LVL_INFO, "Edge registration decider: %s", edge_reg.c_str());
logger->logFmt(LVL_INFO, "graphSLAM Optimizer: %s", optimizer.c_str());
// CGraphSlamHandler initialization
CGraphSlamHandler<GRAPH_T> graphslam_handler(
logger, &options_checker, !disable_visuals.getValue());
graphslam_handler.setFNames(ini_fname, rawlog_fname, ground_truth_fname);
graphslam_handler.initEngine(node_reg, edge_reg, optimizer);
graphslam_handler.printParams();
graphslam_handler.execute();
}
// Main
// ////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
// initializign the logger instance
COutputLogger logger("graphslam-engine_app");
logger.logging_enable_keep_record = true;
try {
bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help");
bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");
// Instance for managing the available graphslam deciders optimizers
TUserOptionsChecker graphslam_opts;
// Input Validation
if (!cmd_line.parse( argc, argv ) || showVersion || showHelp) {
return 0;
}
// fetch the command line graphslam_opts
// ////////////////////////////////////////////////////////////
// decide whether to display the help messages for the deciders/optimizers
{
bool list_registrars = false;
if (list_all_registrars.getValue()) {
graphslam_opts.dumpRegistrarsToConsole("all");
list_registrars = true;
}
if (list_node_registrars.getValue()) {
graphslam_opts.dumpRegistrarsToConsole("node");
list_registrars = true;
}
if (list_edge_registrars.getValue()) {
graphslam_opts.dumpRegistrarsToConsole("edge");
list_registrars = true;
}
if (list_optimizers.getValue()) {
graphslam_opts.dumpOptimizersToConsole();
}
if (list_registrars || list_optimizers.getValue()) {
logger.logFmt(LVL_INFO, "Exiting.. ");
return 0;
}
}
// fetch which registration deciders / optimizer to use
string node_reg = arg_node_reg.getValue();
string edge_reg = arg_edge_reg.getValue();
string optimizer = arg_optimizer.getValue();
ASSERTMSG_(graphslam_opts.checkRegistrationDeciderExists(node_reg, "node"),
format("\nNode Registration Decider %s is not available.\n",
node_reg.c_str()) );
ASSERTMSG_(graphslam_opts.checkRegistrationDeciderExists(edge_reg, "edge"),
format("\nEdge Registration Decider %s is not available.\n",
edge_reg.c_str()) );
ASSERTMSG_(graphslam_opts.checkOptimizerExists(optimizer),
format("\nOptimizer %s is not available\n",
optimizer.c_str()) );
// fetch the filenames
// ini file
string ini_fname = arg_ini_file.getValue();
// rawlog file
string rawlog_fname = arg_rawlog_file.getValue();
// ground-truth file
string ground_truth_fname;
if ( arg_ground_truth_file.isSet() ) {
ground_truth_fname = arg_ground_truth_file.getValue();
}
if (disable_visuals.getValue()) { // enabling Visualization objects
logger.logFmt(LVL_WARN, "Running on headless mode - Visuals disabled");
}
logger.logFmt(LVL_INFO, "Node registration decider: %s", node_reg.c_str());
logger.logFmt(LVL_INFO, "Edge registration decider: %s", edge_reg.c_str());
logger.logFmt(LVL_INFO, "graphSLAM Optimizer: %s", optimizer.c_str());
// CGraphSlamHandler initialization
CGraphSlamHandler graphslam_handler;
graphslam_handler.setOutputLoggerPtr(&logger);
graphslam_handler.readConfigFname(ini_fname);
graphslam_handler.setRawlogFname(rawlog_fname);
// Visuals initialization
if (!disable_visuals.getValue()) {
graphslam_handler.initVisualization();
}
// CGraphSlamEngine initialization
CGraphSlamEngine<CNetworkOfPoses2DInf> graphslam_engine(
ini_fname,
rawlog_fname,
ground_truth_fname,
graphslam_handler.win_manager,
graphslam_opts.node_regs_map[node_reg](),
graphslam_opts.edge_regs_map[edge_reg](),
graphslam_opts.optimizers_map[optimizer]());
// print the problem parameters
graphslam_handler.printParams();
graphslam_engine.printParams();
// Variables initialization
CFileGZInputStream rawlog_stream(rawlog_fname);
CActionCollectionPtr action;
CSensoryFramePtr observations;
CObservationPtr observation;
size_t curr_rawlog_entry;
// Read the dataset and pass the measurements to CGraphSlamEngine
bool cont_exec = true;
while (CRawlog::getActionObservationPairOrObservation(
rawlog_stream,
action,
observations,
observation,
curr_rawlog_entry) && cont_exec) {
// actual call to the graphSLAM execution method
// Exit if user pressed C-c
cont_exec = graphslam_engine.execGraphSlamStep(
action,
observations,
observation,
curr_rawlog_entry);
}
logger.logFmt(LVL_WARN, "Finished graphslam execution.");
//
// Postprocessing
//
logger.logFmt(LVL_INFO, "Generating overall report...");
graphslam_engine.generateReportFiles(graphslam_handler.output_dir_fname);
// save the graph and the 3DScene
if (graphslam_handler.save_graph) {
std::string save_graph_fname =
graphslam_handler.output_dir_fname +
"/" +
graphslam_handler.save_graph_fname;
graphslam_engine.saveGraph(&save_graph_fname);
}
if (!disable_visuals.getValue() && graphslam_handler.save_3DScene) {
std::string save_3DScene_fname =
graphslam_handler.output_dir_fname +
"/" +
graphslam_handler.save_3DScene_fname;
graphslam_engine.save3DScene(&save_3DScene_fname);
}
// get the occupancy gridmap that was built
if (graphslam_handler.save_gridmap) {
COccupancyGridMap2D gridmap;
graphslam_engine.getOccupancyGridMap2D(&gridmap);
gridmap.saveMetricMapRepresentationToFile(
graphslam_handler.output_dir_fname +
"/" +
graphslam_handler.save_gridmap_fname);
}
////////////////////////////////////////////////////////////////////////
}
catch (exception& e) {
logger.logFmt(LVL_ERROR, "Program finished for an exception!!\n%s\n",
e.what());
mrpt::system::pause();
return -1;
}
catch (...) {
logger.logFmt(LVL_ERROR, "Program finished for an untyped exception!!");
mrpt::system::pause();
return -1;
}
return 0;
}
static std::unique_ptr<cMemorySettingsRepository> ParseArguments(int argc, char ** argv)
{
try
{
// Parse the comand line args:
TCLAP::CmdLine cmd("Cuberite");
TCLAP::ValueArg<int> slotsArg ("s", "max-players", "Maximum number of slots for the server to use, overrides setting in setting.ini", false, -1, "number", cmd);
TCLAP::MultiArg<int> portsArg ("p", "port", "The port number the server should listen to", false, "port", cmd);
TCLAP::SwitchArg commLogArg ("", "log-comm", "Log server client communications to file", cmd);
TCLAP::SwitchArg commLogInArg ("", "log-comm-in", "Log inbound server client communications to file", cmd);
TCLAP::SwitchArg commLogOutArg ("", "log-comm-out", "Log outbound server client communications to file", cmd);
TCLAP::SwitchArg crashDumpFull ("", "crash-dump-full", "Crashdumps created by the server will contain full server memory", cmd);
TCLAP::SwitchArg crashDumpGlobals("", "crash-dump-globals", "Crashdumps created by the server will contain the global variables' values", cmd);
TCLAP::SwitchArg noBufArg ("", "no-output-buffering", "Disable output buffering", cmd);
TCLAP::SwitchArg runAsServiceArg ("d", "service", "Run as a service on Windows, or daemon on UNIX like systems", cmd);
cmd.parse(argc, argv);
// Copy the parsed args' values into a settings repository:
auto repo = cpp14::make_unique<cMemorySettingsRepository>();
if (slotsArg.isSet())
{
int slots = slotsArg.getValue();
repo->AddValue("Server", "MaxPlayers", static_cast<Int64>(slots));
}
if (portsArg.isSet())
{
for (auto port: portsArg.getValue())
{
repo->AddValue("Server", "Port", static_cast<Int64>(port));
}
}
if (commLogArg.getValue())
{
g_ShouldLogCommIn = true;
g_ShouldLogCommOut = true;
}
else
{
g_ShouldLogCommIn = commLogInArg.getValue();
g_ShouldLogCommOut = commLogOutArg.getValue();
}
if (noBufArg.getValue())
{
setvbuf(stdout, nullptr, _IONBF, 0);
}
repo->SetReadOnly();
// Set the service flag directly to cRoot:
if (runAsServiceArg.getValue())
{
cRoot::m_RunAsService = true;
}
// Apply the CrashDump flags for platforms that support them:
#if defined(_WIN32) && !defined(_WIN64) && defined(_MSC_VER) // 32-bit Windows app compiled in MSVC
if (crashDumpGlobals.getValue())
{
g_DumpFlags = static_cast<MINIDUMP_TYPE>(g_DumpFlags | MiniDumpWithDataSegs);
}
if (crashDumpFull.getValue())
{
g_DumpFlags = static_cast<MINIDUMP_TYPE>(g_DumpFlags | MiniDumpWithFullMemory);
}
#endif // 32-bit Windows app compiled in MSVC
return repo;
}
catch (const TCLAP::ArgException & exc)
{
printf("Error reading command line %s for arg %s", exc.error().c_str(), exc.argId().c_str());
return cpp14::make_unique<cMemorySettingsRepository>();
}
}