/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "navlog-viewer-ui.h" #include #include #include #include #include #include #include #include #include #include #include // intersect() #include #include #include #include #include #include #include #include #include #include #include #include // replace() #include // Configure look: static const mrpt::opengl::TFontParams& getFontParams() { static mrpt::opengl::TFontParams fp; static bool init = false; if (!init) { fp.vfont_style = mrpt::opengl::FILL; fp.vfont_name = "mono"; fp.vfont_scale = 10.0f; // pixels fp.draw_shadow = true; } return fp; } // Font size & line spaces for GUI-overlayed text lines static const float Ay = getFontParams().vfont_scale + 3; #define ADD_WIN_TEXTMSG_COL(__MSG, __COL) \ { \ auto fp = getFontParams(); \ fp.color = __COL; \ m_win->background_scene->getViewport()->addTextMessage( \ 5.0, 5 + (lineY++) * Ay, __MSG, unique_id++, fp); \ } #define ADD_WIN_TEXTMSG(__MSG) ADD_WIN_TEXTMSG_COL(__MSG, mrpt::img::TColorf(1, 1, 1)) using namespace std; using namespace mrpt; using namespace mrpt::math; using namespace mrpt::img; using namespace mrpt::maps; using namespace mrpt::serialization; using namespace mrpt::config; using namespace mrpt::poses; using namespace mrpt::gui; using namespace mrpt::nav; NavlogViewerApp::NavlogViewerApp() { // Create main window: mrpt::gui::CDisplayWindowGUI_Params cp; cp.maximized = true; // Create GUI: m_win = mrpt::gui::CDisplayWindowGUI::Create("navlog-viewer", 900, 700, cp); // Add main control window: // ----------------------------- m_winMain = m_win->createManagedSubWindow("Control"); m_winMain->setPosition(nanogui::Vector2i(10, 10)); m_winMain->setLayout(new nanogui::GroupLayout(5 /*margin*/, 5 /*spacing*/)); m_winMain->setFixedWidth(540); { nanogui::Widget* panel = m_winMain->add(); panel->setLayout(new nanogui::BoxLayout( nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 5 /*margin*/)); panel->add("Load...", ENTYPO_ICON_UPLOAD) ->setCallback([this]() { this->OnbtnLoadClick(); }); m_btnPlay = panel->add("Play", ENTYPO_ICON_CONTROLLER_PLAY); m_btnStop = panel->add("Stop", ENTYPO_ICON_CONTROLLER_STOP); panel->add(" Animation delay (s):"); nanogui::TextBox* edPlayInterval = panel->add("0.100"); edPlayInterval->setEditable(true); edPlayInterval->setFormat("[0-9.e+\\-]*"); m_btnPlay->setCallback( [edPlayInterval, this]() { // Rewind if we are at the end: if (std::abs(slidLog->value() - slidLog->range().second) < 1e-6) slidLog->setValue(0); m_autoPlayEnabled = true; m_btnPlay->setEnabled(false); m_btnStop->setEnabled(true); m_btnPlay->setPushed(true); m_btnStop->setPushed(false); m_autoPlayInterval = std::stod(edPlayInterval->value()); }); m_btnStop->setCallback( [this]() { m_autoPlayEnabled = false; m_btnPlay->setEnabled(true); m_btnStop->setEnabled(false); m_btnPlay->setPushed(false); m_btnStop->setPushed(false); }); m_btnStop->setEnabled(false); panel->add("Quit", ENTYPO_ICON_ARROW_RIGHT) ->setCallback([this]() { m_win->setVisible(false); }); } // ===== Time slider: m_txtTimeIndex = m_winMain->add("Time index:"); slidLog = m_winMain->add(); slidLog->setCallback([this](float /*v*/) { updateVisualization(); }); m_tabWidget = m_winMain->add(); // ===== TAB: Information { nanogui::Widget* layer = m_tabWidget->createTab("Information"); auto layout = new nanogui::GridLayout(nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5); layer->setLayout(layout); layer->add("Log entries:"); txtLogEntries = layer->add(" "); layer->add("Log duration:"); txtLogDuration = layer->add(" "); layer->add("Selected PTG index:"); txtSelectedPTG = layer->add(" "); layer->add(" "); // spacer } // ===== TAB: View options { nanogui::Widget* layer = m_tabWidget->createTab("View options"); auto layout = new nanogui::GridLayout(nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5); layer->setLayout(layout); m_cbUseOdometryCoords = layer->add("Use raw odometry"); m_cbUseOdometryCoords->setCallback([this](bool) { updateVisualization(); }); m_cbGlobalFrame = layer->add("In global coordinates"); m_cbGlobalFrame->setChecked(true); m_cbGlobalFrame->setCallback([this](bool) { updateVisualization(); }); m_cbShowDelays = layer->add("Show delays model-based poses"); m_cbShowDelays->setChecked(true); m_cbShowDelays->setCallback([this](bool) { updateVisualization(); }); m_cbDrawShape = layer->add("Draw robot shape on trajectories"); m_cbDrawShape->setChecked(true); m_cbDrawShape->setCallback([this](bool) { updateVisualization(); }); m_cbShowAllDebugFields = layer->add("Show all debug fields"); m_cbShowAllDebugFields->setCallback([this](bool) { updateVisualization(); }); m_cbShowCursor = layer->add("Mouse coordinates"); m_cbShowCursor->setChecked(m_showCursorXY); m_cbShowCursor->setCallback([this](bool v) { m_showCursorXY = v; }); m_ClearanceOverPath = layer->add("Clearance: path/!pointwise"); m_ClearanceOverPath->setCallback([this](bool) { updateVisualization(); }); { auto* panel = layer->add(); panel->setLayout( new nanogui::BoxLayout(nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 0)); panel->add("Min. dist. robot shapes:"); edShapeMinDist = panel->add("1.0"); edShapeMinDist->setEditable(true); edShapeMinDist->setFormat("[0-9.]*"); } m_cbOrtho2DView = layer->add("Orthogonal 2D mode"); m_cbOrtho2DView->setChecked(true); // No need to catch callbacks: the checkbox is checked in the GUI main // loop. { auto* panel = layer->add(); panel->setLayout( new nanogui::BoxLayout(nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 0)); panel->add("Show past poses:"); edPastPoses = panel->add("10"); edPastPoses->setEditable(true); edPastPoses->setFormat("[0-9]*"); } layer->add("Show for each PTG:"); const auto lst = std::vector({"TP-Obstacles only", "+ final scores", "+ preliminary scores"}); m_rbPerPTGPlots = layer->add(lst, lst); m_rbPerPTGPlots->setCallback([this](int) { updateVisualization(); }); m_rbPerPTGPlots->setSelectedIndex(2); } // ===== TAB: Advanced { nanogui::Widget* layer = m_tabWidget->createTab("Advanced"); auto layout = new nanogui::GridLayout(nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 10); layer->setLayout(layout); auto lambdaAddSmallFontBtn = [](nanogui::Widget* parent, const std::string& caption, const std::function& callback, int icon = 0) { auto btn = parent->add(caption, icon); btn->setCallback(callback); btn->setFontSize(16); }; lambdaAddSmallFontBtn(layer, "See PTGs details", [this]() { OnmnuSeePTGParamsSelected(); }); lambdaAddSmallFontBtn( layer, "Save current obstacle map...", [this]() { OnmnuSaveCurrentObstacles(); }); lambdaAddSmallFontBtn( layer, "Export map plot to MATLAB...", [this]() { OnmnuMatlabPlotsSelected(); }); lambdaAddSmallFontBtn( layer, "Export paths info to MATLAB...", [this]() { OnmnuMatlabExportPaths(); }); lambdaAddSmallFontBtn( layer, "Save score matrices...", [this]() { OnmnuSaveScoreMatrixSelected(); }); lambdaAddSmallFontBtn( layer, "Export current log entry...", [this]() { OnmnuExportSelected(); }); lambdaAddSmallFontBtn( layer, "Localization vs odom report...", [this]() { OnmnuGenerateOdometryVsLocalizationReport(); }); } // ===== TAB: Manually pick trajectory { nanogui::Widget* layer = m_tabWidget->createTab("Manual pick"); auto layout = new nanogui::GridLayout(nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 10); layer->setLayout(layout); m_cbManualPickMode = layer->add("Enable manual pick"); m_cbManualPickMode->setCallback([this](bool) { updateVisualization(); }); layer->add(" "); layer->add("PTG index:"); m_slidPtgIndex = layer->add(); m_slidPtgIndex->setCallback([this](float /*v*/) { updateVisualization(); }); layer->add("Trajectory index:"); m_slidTrajectoryIndex = layer->add(); m_slidTrajectoryIndex->setCallback([this](float /*v*/) { updateVisualization(); }); m_manualPickLabel = layer->add("Selection: None"); layer->add(" "); layer->add(" "); m_btnManualPickAppendYaml = layer->add("Append to 'nav-selected.yaml'"); m_btnManualPickAppendYaml->setCallback([this]() { OnManualPickAppendYaml(); }); } m_tabWidget->setActiveTab(0); // Add a background scene: // ----------------------------- { mrpt::opengl::Scene::Ptr scene = mrpt::opengl::Scene::Create(); m_win->camera().setAzimuthDegrees(-90.0f); m_win->camera().setElevationDegrees(90.0f); m_win->camera().setZoomDistance(25.0f); { auto vi = scene->createViewport("xyz"); vi->setViewportPosition(-0.1, 0.0, 0.1, 0.1); vi->setTransparent(true); auto glCorners = mrpt::opengl::stock_objects::CornerXYZSimple(1.0f, 1.0f); scene->insert(glCorners, "xyz"); } // XY ground plane: mrpt::opengl::CGridPlaneXY::Ptr gl_grid = mrpt::opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1, 0.75f); gl_grid->setColor_u8(mrpt::img::TColor(0xa0a0a0, 0x90)); scene->insert(gl_grid); // XYZ corner at origin: scene->insert(mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.0)); auto lck = mrpt::lockHelper(m_win->background_scene_mtx); m_win->background_scene = std::move(scene); } // Setup idle loop code: // ----------------------------- m_win->addLoopCallback([this]() { OnMainIdleLoop(); }); m_win->addKeyboardCallback([this](int key, int scancode, int action, int modifiers) { return OnKeyboardCallback(key, scancode, action, modifiers); }); // Update view and show window: // ----------------------------- m_win->performLayout(); m_win->drawAll(); m_win->setVisible(true); // Disable some controls, etc.. updateInfoFromLoadedLog(); } void NavlogViewerApp::OnbtnLoadClick() { NANOGUI_START_TRY const std::string fileName = nanogui::file_dialog( { {"reactivenavlog", "Navigation logs"}, { "navlog", "Navigation logs"} }, false /*load*/); if (fileName.empty()) return; loadLogfile(fileName); NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::loadLogfile(const std::string& fileName) { using namespace std::string_literals; NANOGUI_START_TRY m_openedFileName = fileName; m_winMain->setTitle("Control ["s + mrpt::system::extractFileName(fileName) + "]"s); mrpt::io::CCompressedInputStream f(fileName); m_logdata.clear(); m_logdata_ptg_paths.clear(); m_pastPoses.clear(); mrpt::rtti::CListOfClasses validClasses; validClasses.insert(CLASS_ID(mrpt::nav::CLogFileRecord)); m_log_first_tim = INVALID_TIMESTAMP; m_log_last_tim = INVALID_TIMESTAMP; auto arch = archiveFrom(f); for (;;) { try { CSerializable::Ptr obj = arch.ReadObject(); if (!validClasses.contains(obj->GetRuntimeClass())) { // auto dlg = new nanogui::MessageDialog( m_win.get(), nanogui::MessageDialog::Type::Warning, "Error loading navlog file", mrpt::format("Unexpected class found: %s", obj->GetRuntimeClass()->className)); break; } m_logdata.push_back(obj); // generate time stats: if (IS_CLASS(*obj, CLogFileRecord)) { const CLogFileRecord::Ptr logptr = std::dynamic_pointer_cast(obj); const auto it = logptr->timestamps.find("tim_start_iteration"); if (it != logptr->timestamps.end()) m_log_last_tim = it->second; if (!logptr->infoPerPTG.empty()) { size_t nPTGs = logptr->infoPerPTG.size(); if (nPTGs > m_logdata_ptg_paths.size()) { m_logdata_ptg_paths.resize(nPTGs); for (size_t i = 0; i < nPTGs; i++) { if (logptr->infoPerPTG[i].ptg) m_logdata_ptg_paths[i] = logptr->infoPerPTG[i].ptg; } } else { for (size_t i = 0; i < nPTGs; i++) logptr->infoPerPTG[i].ptg = m_logdata_ptg_paths[i]; } } } if (m_log_first_tim == INVALID_TIMESTAMP && m_log_last_tim != INVALID_TIMESTAMP) m_log_first_tim = m_log_last_tim; } catch (CExceptionEOF&) { break; } catch (const std::exception& e) { // EOF in the middle of an object... It may be usual if the logger // is shut down not cleanly. // Show the error message (catch below) throw; } } NANOGUI_END_TRY(*m_win) // Update stats, etc... updateInfoFromLoadedLog(); } void NavlogViewerApp::OnMainIdleLoop() { // Idle loop tasks: // Auto play: if (m_autoPlayEnabled && m_autoPlayTimer.Tac() >= m_autoPlayInterval) { m_autoPlayTimer.Tic(); float p = slidLog->value(); if ((p + 1) <= slidLog->range().second) { slidLog->setValue(p + 1); updateVisualization(); } else { m_btnStop->callback()(); } } if (m_showCursorXY) { OntimMouseXY(); } // Restrict camera motion: if (m_win->background_scene) { auto& mainCam = m_win->camera(); if (m_cbOrtho2DView->checked()) { mainCam.setElevationDegrees(90); mainCam.setAzimuthDegrees(-90); } mainCam.setCameraProjective(!m_cbOrtho2DView->checked()); } // Copy camera orientation from the main window into the small XYZ view: if (m_win && m_win->background_scene) { if (auto view = m_win->background_scene->getViewport("xyz"); view) { auto& mainCam = m_win->camera(); auto& xyzCam = view->getCamera(); xyzCam.setAzimuthDegrees(mainCam.getAzimuthDegrees()); xyzCam.setElevationDegrees(mainCam.getElevationDegrees()); xyzCam.setOrthogonal(true); xyzCam.setZoomDistance(2.0); } } } bool NavlogViewerApp::OnKeyboardCallback( int key, [[maybe_unused]] int scancode, int action, [[maybe_unused]] int modifiers) { if (action != GLFW_PRESS && action != GLFW_REPEAT) return false; switch (key) { case GLFW_KEY_LEFT: { float p = slidLog->value(); if ((p - 1) >= slidLog->range().first) { slidLog->setValue(p - 1); updateVisualization(); } } break; case GLFW_KEY_RIGHT: { float p = slidLog->value(); if ((p + 1) <= slidLog->range().second) { slidLog->setValue(p + 1); updateVisualization(); } } break; }; return false; } void NavlogViewerApp::updateInfoFromLoadedLog() { const size_t N = m_logdata.size(); if (N > 0) { this->txtLogEntries->setValue(std::to_string(N)); this->slidLog->setRange({0, N - 1}); this->slidLog->setValue(0); updateVisualization(); } std::string sDuration("???"); if (m_log_first_tim != INVALID_TIMESTAMP && m_log_last_tim != INVALID_TIMESTAMP) { sDuration = mrpt::system::intervalFormat(mrpt::system::timeDifference(m_log_first_tim, m_log_last_tim)); } txtLogDuration->setValue(sDuration); // m_win->performLayout(); } // --------------------------------------------- // DRAW ONE LOG RECORD // --------------------------------------------- void NavlogViewerApp::updateVisualization() { NANOGUI_START_TRY using namespace std::string_literals; updateManualPickControls(); const int log_idx = mrpt::round(slidLog->value()); if (log_idx >= int(m_logdata.size())) return; // In the future, we could handle more log classes. For now, only // "CLogFileRecord::Ptr": auto logptr = std::dynamic_pointer_cast(m_logdata[log_idx]); const CLogFileRecord& log = *logptr; txtSelectedPTG->setValue( std::to_string(log.nSelectedPTG) + "from [0-"s + std::to_string(log.nPTGs - 1) + "]"s); { std::string timeIndexStr = "Time index: "s + std::to_string(log_idx) + " / "s + std::to_string(std::max(1U, m_logdata.size()) - 1); if (auto itTim = log.timestamps.find("tim_start_iteration"); itTim != log.timestamps.end()) { timeIndexStr += " (tim_start_iteration="s + mrpt::system::dateTimeLocalToString(itTim->second) + " [local time])"s; } m_txtTimeIndex->setCaption(timeIndexStr); } const bool is_NOP_cmd = log.ptg_index_NOP >= 0; const int sel_ptg_idx = !is_NOP_cmd ? log.nSelectedPTG : log.ptg_index_NOP; // Draw WS-obstacles // -------------------------------- // Update 3D view: { auto lck = mrpt::lockHelper(m_win->background_scene_mtx); mrpt::opengl::Scene::Ptr& scene = m_win->background_scene; const CVectorFloat shap_x = log.robotShape_x, shap_y = log.robotShape_y; // Robot frame of reference: mrpt::opengl::CSetOfObjects::Ptr gl_robot_frame; { mrpt::opengl::CRenderizable::Ptr gl_rbframe_r = scene->getByName("robot_frame"); // Get or create if new if (!gl_rbframe_r) { gl_robot_frame = mrpt::opengl::CSetOfObjects::Create(); gl_robot_frame->setName("robot_frame"); scene->insert(gl_robot_frame); } else { gl_robot_frame = std::dynamic_pointer_cast(gl_rbframe_r); } // Global or local coordinates? if (m_cbUseOdometryCoords->checked()) { // Use odometry pose increment wrt initial instant, // taking the localization-based "good" pose of the initial // instant as a reference, such that the obtained // coordinates closely match those expected in the "map" // frame: auto log0ptr = mrpt::ptr_cast::from(m_logdata[0]); ASSERT_(log0ptr.get()); const auto curPose = log0ptr->robotPoseLocalization + (log.robotPoseOdometry - log0ptr->robotPoseOdometry); gl_robot_frame->setPose(curPose); m_pastPoses.push_back(curPose); } else if (m_cbGlobalFrame->checked()) { gl_robot_frame->setPose( mrpt::poses::CPose3D(mrpt::poses::CPose2D(log.robotPoseLocalization))); m_pastPoses.push_back(log.robotPoseLocalization); // Move the window focus: auto& cam = m_win->camera(); const float px = cam.getCameraPointingX(); const float py = cam.getCameraPointingY(); const float cam_zoom = cam.getZoomDistance(); if ((mrpt::math::TPoint2D(log.robotPoseLocalization) - mrpt::math::TPoint2D(px, py)) .norm() > .3 * cam_zoom) cam.setCameraPointing(log.robotPoseLocalization.x, log.robotPoseLocalization.y, 0.0); } else { gl_robot_frame->setPose(mrpt::poses::CPose3D()); m_pastPoses.clear(); } } // custom visuals: { mrpt::opengl::CSetOfObjects::Ptr gl_custom_vizs; { // Get or create if new auto gl_r = gl_robot_frame->getByName("custom_viz"); if (!gl_r) { gl_custom_vizs = mrpt::opengl::CSetOfObjects::Create(); gl_custom_vizs->setName("custom_viz"); gl_robot_frame->insert(gl_custom_vizs); } else { gl_custom_vizs = std::dynamic_pointer_cast(gl_r); } gl_custom_vizs->clear(); for (const auto& viz : log.visuals) gl_custom_vizs->insert(viz); } } // Extrapolated poses from delay models: { mrpt::opengl::CSetOfObjects::Ptr gl_relposes; mrpt::opengl::CRenderizable::Ptr gl_relposes_r = gl_robot_frame->getByName("relposes"); // Get or create if new if (!gl_relposes_r) { gl_relposes = mrpt::opengl::CSetOfObjects::Create(); gl_relposes->setName("relposes"); gl_robot_frame->insert(gl_relposes); } else { gl_relposes = std::dynamic_pointer_cast(gl_relposes_r); } gl_relposes->clear(); if (m_cbShowDelays->checked()) { { mrpt::opengl::CSetOfObjects::Ptr gl_relpose_sense = mrpt::opengl::stock_objects::CornerXYSimple(0.3f, 1); gl_relpose_sense->setName("sense"); gl_relpose_sense->enableShowName(true); gl_relpose_sense->setPose(log.relPoseSense); gl_relposes->insert(gl_relpose_sense); } { mrpt::opengl::CSetOfObjects::Ptr gl_relpose_cmdvel = mrpt::opengl::stock_objects::CornerXYSimple(0.3f, 1); gl_relpose_cmdvel->setName("cmdVel"); gl_relpose_cmdvel->enableShowName(true); gl_relpose_cmdvel->setPose(log.relPoseVelCmd); gl_relposes->insert(gl_relpose_cmdvel); } } } // track of past poses: if (!m_pastPoses.empty()) { mrpt::opengl::CSetOfObjects::Ptr gl_poses; mrpt::opengl::CRenderizable::Ptr gl_track = scene->getByName("past_poses"); // Get or create if new if (!gl_track) { gl_poses = mrpt::opengl::CSetOfObjects::Create(); gl_poses->setName("past_poses"); scene->insert(gl_poses); } else { gl_poses = dynamic_pointer_cast(gl_track); } ASSERT_(gl_poses); gl_poses->clear(); for (const auto& p : m_pastPoses) { auto glCorner = mrpt::opengl::stock_objects::CornerXYSimple(0.3f, 1.0f); glCorner->setPose(p); gl_poses->insert(glCorner); } } const size_t maxPastPoses = std::stol(edPastPoses->value()); while (!m_pastPoses.empty() && m_pastPoses.size() > maxPastPoses) { m_pastPoses.erase(m_pastPoses.begin()); } { // Obstacles: original mrpt::opengl::CPointCloud::Ptr gl_obs; mrpt::opengl::CRenderizable::Ptr gl_obs_r = gl_robot_frame->getByName("obs-raw"); // Get or create if new if (!gl_obs_r) { gl_obs = mrpt::opengl::CPointCloud::Create(); gl_obs->setName("obs-raw"); gl_obs->setPointSize(3); gl_obs->setColor_u8(mrpt::img::TColor(0xff, 0xff, 0x00)); gl_robot_frame->insert(gl_obs); } else { gl_obs = dynamic_pointer_cast(gl_obs_r); } gl_obs->loadFromPointsMap(&log.WS_Obstacles_original); if (m_cbShowDelays->checked()) gl_obs->setPose(log.relPoseSense); else gl_obs->setPose(mrpt::poses::CPose3D()); } { // Obstacles: after optional filtering mrpt::opengl::CPointCloud::Ptr gl_obs; mrpt::opengl::CRenderizable::Ptr gl_obs_r = gl_robot_frame->getByName("obs"); // Get or create if new if (!gl_obs_r) { gl_obs = mrpt::opengl::CPointCloud::Create(); gl_obs->setName("obs"); gl_obs->setPointSize(1.5); gl_obs->setLocation(0, 0, 0.001); gl_obs->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff)); gl_robot_frame->insert(gl_obs); } else { gl_obs = std::dynamic_pointer_cast(gl_obs_r); } gl_obs->loadFromPointsMap(&log.WS_Obstacles); if (m_cbShowDelays->checked()) gl_obs->setPose(log.relPoseSense); else gl_obs->setPose(mrpt::poses::CPose3D()); } { // Selected PTG path: mrpt::opengl::CSetOfLines::Ptr gl_path; mrpt::opengl::CRenderizable::Ptr gl_path_r = gl_robot_frame->getByName("path"); // Get or create if new if (!gl_path_r) { gl_path = mrpt::opengl::CSetOfLines::Create(); gl_path->setName("path"); gl_path->setLineWidth(2.0); gl_path->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00)); gl_robot_frame->insert(gl_path); } else gl_path = mrpt::ptr_cast::from(gl_path_r); gl_path->clear(); if (sel_ptg_idx < int(m_logdata_ptg_paths.size()) && sel_ptg_idx >= 0) { mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg = m_logdata_ptg_paths[sel_ptg_idx]; if (ptg) { if (!ptg->isInitialized()) ptg->initialize(); auto& ipp = log.infoPerPTG[sel_ptg_idx]; // Set instantaneous dyn state: ptg->updateNavDynamicState(is_NOP_cmd ? ipp.lastDynState : ipp.dynState); // Draw path: const int selected_k = log.ptg_index_NOP < 0 ? ptg->alpha2index(ipp.desiredDirection) : log.ptg_last_k_NOP; float max_dist = ptg->getRefDistance(); ptg->add_robotShape_to_setOfLines(*gl_path); ptg->renderPathAsSimpleLine(selected_k, *gl_path, 0.10, max_dist); // PTG origin: // enable delays model? mrpt::math::TPose2D ptg_origin = (m_cbShowDelays->checked()) ? log.relPoseVelCmd : mrpt::math::TPose2D(0, 0, 0); // "NOP cmd" case: if (log.ptg_index_NOP >= 0) { ptg_origin = ptg_origin - log.rel_cur_pose_wrt_last_vel_cmd_NOP; } gl_path->setPose(ptg_origin); // Overlay a sequence of robot shapes: if (m_cbDrawShape->checked()) { double min_shape_dists = std::max(0.01, std::stod(edShapeMinDist->value())); for (double d = min_shape_dists; d < max_dist; d += min_shape_dists) { uint32_t step; if (!ptg->getPathStepForDist(selected_k, d, step)) continue; const auto p = ptg->getPathPose(selected_k, step); ptg->add_robotShape_to_setOfLines(*gl_path, mrpt::poses::CPose2D(p)); } } { // Robot shape: mrpt::opengl::CSetOfLines::Ptr gl_shape; mrpt::opengl::CRenderizable::Ptr gl_shape_r = gl_robot_frame->getByName("shape"); // Get or create if new if (!gl_shape_r) { gl_shape = std::make_shared(); gl_shape->setName("shape"); gl_shape->setLineWidth(4.0); gl_shape->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00)); gl_robot_frame->insert(gl_shape); } else { gl_shape = std::dynamic_pointer_cast(gl_shape_r); } gl_shape->clear(); ptg->add_robotShape_to_setOfLines(*gl_shape); } { mrpt::opengl::CSetOfLines::Ptr gl_shape; mrpt::opengl::CRenderizable::Ptr gl_shape_r = gl_robot_frame->getByName("velocity"); // Get or create if new if (!gl_shape_r) { gl_shape = std::make_shared(); gl_shape->setName("velocity"); gl_shape->setLineWidth(4.0); gl_shape->setColor_u8(mrpt::img::TColor(0x00, 0xff, 0xff)); gl_robot_frame->insert(gl_shape); } else { gl_shape = std::dynamic_pointer_cast(gl_shape_r); } gl_shape->clear(); const mrpt::math::TTwist2D& velLocal = log.cur_vel_local; gl_shape->appendLine(0, 0, 0, velLocal.vx, velLocal.vy, 0); } } } } { // Manually-picked PTG path: mrpt::opengl::CSetOfLines::Ptr gl_path; mrpt::opengl::CRenderizable::Ptr gl_path_r = gl_robot_frame->getByName("manual-path"); // Get or create if new if (!gl_path_r) { gl_path = mrpt::opengl::CSetOfLines::Create(); gl_path->setName("manual-path"); gl_path->setLineWidth(2.0); gl_path->setColor_u8(mrpt::img::TColor(0x00, 0xff, 0x00)); gl_robot_frame->insert(gl_path); } else gl_path = mrpt::ptr_cast::from(gl_path_r); gl_path->clear(); if (m_manualPickPTGIdx >= 0 && m_manualPickPTGIdx < int(m_logdata_ptg_paths.size()) && m_manualPickTrajectoryIdx >= 0) { mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg = m_logdata_ptg_paths[m_manualPickPTGIdx]; if (ptg) { if (!ptg->isInitialized()) ptg->initialize(); const bool this_NOP_cmd = m_manualPickPTGIdx == static_cast(m_logdata_ptg_paths.size() - 1); auto& ipp = log.infoPerPTG[m_manualPickPTGIdx]; // Set instantaneous dyn state: ptg->updateNavDynamicState(this_NOP_cmd ? ipp.lastDynState : ipp.dynState); // Draw path: const int selected_k = m_manualPickTrajectoryIdx; float max_dist = ptg->getRefDistance(); ptg->add_robotShape_to_setOfLines(*gl_path); ptg->renderPathAsSimpleLine(selected_k, *gl_path, 0.10, max_dist); // PTG origin: // enable delays model? mrpt::math::TPose2D ptg_origin = (m_cbShowDelays->checked()) ? log.relPoseVelCmd : mrpt::math::TPose2D(0, 0, 0); // "NOP cmd" case: if (log.ptg_index_NOP >= 0) { ptg_origin = ptg_origin - log.rel_cur_pose_wrt_last_vel_cmd_NOP; } gl_path->setPose(ptg_origin); // Overlay a sequence of robot shapes: if (m_cbDrawShape->checked()) { double min_shape_dists = std::max(0.01, std::stod(edShapeMinDist->value())); for (double d = min_shape_dists; d < max_dist; d += min_shape_dists) { uint32_t step; if (!ptg->getPathStepForDist(selected_k, d, step)) continue; const auto p = ptg->getPathPose(selected_k, step); ptg->add_robotShape_to_setOfLines(*gl_path, mrpt::poses::CPose2D(p)); } } { // Robot shape: mrpt::opengl::CSetOfLines::Ptr gl_shape; mrpt::opengl::CRenderizable::Ptr gl_shape_r = gl_robot_frame->getByName("shape"); // Get or create if new if (!gl_shape_r) { gl_shape = std::make_shared(); gl_shape->setName("shape"); gl_shape->setLineWidth(4.0); gl_shape->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00)); gl_robot_frame->insert(gl_shape); } else { gl_shape = std::dynamic_pointer_cast(gl_shape_r); } gl_shape->clear(); ptg->add_robotShape_to_setOfLines(*gl_shape); } } } } { // Target: mrpt::opengl::CSetOfObjects::Ptr gl_trgCorners; mrpt::opengl::CRenderizable::Ptr gl_trgC_r = gl_robot_frame->getByName("target_corners"); // Get or create if new mrpt::opengl::CPointCloud::Ptr gl_trg; mrpt::opengl::CRenderizable::Ptr gl_trg_r = gl_robot_frame->getByName("target"); // Get or create if new if (!gl_trg_r) { gl_trg = mrpt::opengl::CPointCloud::Create(); gl_trg->setName("target"); gl_trg->enableShowName(true); gl_trg->setPointSize(9.0); gl_trg->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0x00)); gl_robot_frame->insert(gl_trg); gl_trgCorners = mrpt::opengl::CSetOfObjects::Create(); gl_trgCorners->setName("target_corners"); gl_robot_frame->insert(gl_trgCorners); } else { gl_trg = std::dynamic_pointer_cast(gl_trg_r); gl_trgCorners = std::dynamic_pointer_cast(gl_trgC_r); ASSERT_(gl_trg); ASSERT_(gl_trgCorners); } // Move the map & add a point at (0,0,0) so the name label // appears at the target: gl_trg->clear(); gl_trgCorners->clear(); if (!log.WS_targets_relative.empty()) { const auto t0 = log.WS_targets_relative[0]; const float tz = .05f; gl_trg->setLocation(t0.x, t0.y, tz); for (const auto& t : log.WS_targets_relative) { gl_trg->insertPoint(t.x - t0.x, t.y - t0.y, tz); auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.25f); glCorner->setPose(t); gl_trgCorners->insert(glCorner); } } } } // Show extra info as text msgs: // --------------------------------- int lineY = 0, unique_id = 0; m_win->background_scene->getViewport()->clearTextMessages(); // Mouse position at Z=0 // Updated in timer callback: if (m_cbShowCursor->checked()) { lineY++; unique_id++; } if (m_cbShowAllDebugFields->checked()) { for (const auto& e : log.timestamps) ADD_WIN_TEXTMSG(mrpt::format( "Timestamp %-20s=%s", e.first.c_str(), mrpt::system::dateTimeLocalToString(e.second).c_str())); } { const unsigned int nObsOrg = log.WS_Obstacles_original.size(), nObs = log.WS_Obstacles.size(); const unsigned int nObsFiltered = nObsOrg - nObs; if (nObsFiltered) { ADD_WIN_TEXTMSG(mrpt::format("Obstacle points=%u (%u filtered out)", nObs, nObsFiltered)); } else { ADD_WIN_TEXTMSG(mrpt::format("Obstacle points=%u", nObs)); } } ADD_WIN_TEXTMSG(mrpt::format( "cmd_vel=%s", log.cmd_vel ? log.cmd_vel->asString().c_str() : "NOP (Continue last PTG)")); ADD_WIN_TEXTMSG(mrpt::format( "cur_vel =[%.02f m/s, %0.2f m/s, %.02f dps]", log.cur_vel.vx, log.cur_vel.vy, mrpt::RAD2DEG(log.cur_vel.omega))); ADD_WIN_TEXTMSG(mrpt::format( "cur_vel_local=[%.02f m/s, %0.2f m/s, %.02f dps]", log.cur_vel_local.vx, log.cur_vel_local.vy, mrpt::RAD2DEG(log.cur_vel_local.omega))); { static TPose2D formerPoseLoc = log.robotPoseLocalization; ADD_WIN_TEXTMSG(mrpt::format( "robot_pose =%35s | Increment since last step=%s", log.robotPoseLocalization.asString().c_str(), (log.robotPoseLocalization - formerPoseLoc).asString().c_str())); formerPoseLoc = log.robotPoseLocalization; } { static TPose2D formerPoseOdo = log.robotPoseOdometry; ADD_WIN_TEXTMSG(mrpt::format( "robot_odometry=%35s | Increment since last step=%s", log.robotPoseOdometry.asString().c_str(), (log.robotPoseOdometry - formerPoseOdo).asString().c_str())); formerPoseOdo = log.robotPoseOdometry; } { for (unsigned int i = 0; i < log.WS_targets_relative.size(); i++) { ADD_WIN_TEXTMSG( mrpt::format("rel_target[%u]=%s", i, log.WS_targets_relative[i].asString().c_str())); } } if (log.cmd_vel_original) { std::stringstream ss; ss << "original cmd_vel: "; ss << log.cmd_vel_original->asString(); ADD_WIN_TEXTMSG(ss.str()); } { std::stringstream ss; ss << "Performance: "; for (size_t i = 0; i < log.infoPerPTG.size(); i++) ss << "PTG#" << i << mrpt::format( " TPObs:%ss HoloNav:%ss |", mrpt::system::unitsFormat(log.infoPerPTG[i].timeForTPObsTransformation).c_str(), mrpt::system::unitsFormat(log.infoPerPTG[i].timeForHolonomicMethod).c_str()); ADD_WIN_TEXTMSG(ss.str()); } for (unsigned int nPTG = 0; nPTG < log.infoPerPTG.size(); nPTG++) { const CLogFileRecord::TInfoPerPTG& pI = log.infoPerPTG[nPTG]; mrpt::img::TColorf col; if (((int)nPTG) == log.nSelectedPTG) col = mrpt::img::TColorf(1, 1, 1); else col = mrpt::img::TColorf(.8f, .8f, .8f); std::string sFactors; for (const auto& kv : pI.evalFactors) { sFactors += kv.first; sFactors += ": "; sFactors += std::to_string(kv.second); sFactors += " "; } ADD_WIN_TEXTMSG_COL( mrpt::format( "PTG#%u: SelDir=%+7.01f deg SelSpeed=%.03f Eval=%5.03f. %s", nPTG, mrpt::RAD2DEG(pI.desiredDirection), pI.desiredSpeed, pI.evaluation, sFactors.c_str()), col); } ADD_WIN_TEXTMSG(mrpt::format( "relPoseSense: %s relPoseVelCmd:%s", log.relPoseSense.asString().c_str(), log.relPoseVelCmd.asString().c_str())); if (m_cbShowAllDebugFields->checked()) { if (!log.infoPerPTG.empty()) { ADD_WIN_TEXTMSG(format( "navDynState: curVelLocal=%s relTarget=%s " "targetRelSpeed=%.02f", log.infoPerPTG.front().dynState.curVelLocal.asString().c_str(), log.infoPerPTG.front().dynState.relTarget.asString().c_str(), log.infoPerPTG.front().dynState.targetRelSpeed) .c_str()); } for (const auto& e : log.values) ADD_WIN_TEXTMSG(format( "%-30s=%s ", e.first.c_str(), mrpt::system::unitsFormat(e.second, 3, false).c_str())); for (const auto& e : log.additional_debug_msgs) ADD_WIN_TEXTMSG(format("%-30s=%s ", e.first.c_str(), e.second.c_str())); } // Draw TP-obstacles // -------------------------------- // log.infoPerPTG.size() may be != nPTGs in the last entry is used for // "NOP cmdvel" bool mustPerformLayout = false; for (unsigned int nPTG = 0; nPTG < log.infoPerPTG.size(); nPTG++) { const bool is_selected_ptg = (int(nPTG) == log.nSelectedPTG); VizPTG& viz = m_mywins3D[format("PTG%u", nPTG)]; if (!viz.win) { mustPerformLayout = true; const static int W = 270; const static int H = 270; viz.win = m_win->createManagedSubWindow(format("%u|TP-Obstacles", nPTG)); viz.win->setLayout(new nanogui::GroupLayout(0, 0, 0, 0)); viz.win->setSize({W, H}); viz.win->setPosition({5 + W * (nPTG % 3), 315 + H * (nPTG / 3)}); viz.glCanvas = viz.win->add(); viz.glCanvas->setFixedSize({W, H}); viz.win->buttonPanel() ->add("", ENTYPO_ICON_PLUS) ->setCallback( [this, viz]() { // viz.win->setFixedSize({W, H}); viz.glCanvas->setFixedSize({W, H}); m_win->performLayout(); }); viz.win->buttonPanel() ->add("", ENTYPO_ICON_POPUP) ->setCallback( [this, viz]() { // viz.win->setFixedSize({2 * W, 2 * H}); viz.glCanvas->setFixedSize({2 * W, 2 * H}); m_win->performLayout(); }); viz.glCanvas->scene = mrpt::opengl::Scene::Create(); auto& scene = viz.glCanvas->scene; // win viz.glCanvas->scene->getViewport()->addTextMessage( 4, 4, mrpt::format("[%u]:%s", nPTG, log.infoPerPTG[nPTG].PTG_desc.c_str()), 0 /*id*/, getFontParams()); scene->insert(mrpt::opengl::CGridPlaneXY::Create(-1.0f, 1.0f, -1.0f, 1.0f, .0f, 1.0f)); scene->insert(mrpt::opengl::stock_objects::CornerXYSimple(0.4f, 2.0f)); auto& cam = viz.glCanvas->camera(); cam.setAzimuthDegrees(-90.0f); cam.setElevationDegrees(90.0f); cam.setZoomDistance(4.6f); cam.setCameraProjective(false); { auto gl_obj = mrpt::opengl::CDisk::Create(); gl_obj->setDiskRadius(1.01f, 1.0); gl_obj->setSlicesCount(30); gl_obj->setColor_u8(mrpt::img::TColor(0x30, 0x30, 0x30, 0xff)); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CSetOfLines::Create(); gl_obj->setName("tp_obstacles"); gl_obj->setLineWidth(1.0f); gl_obj->setVerticesPointSize(4.0f); gl_obj->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff, 0xff)); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CSetOfLines::Create(); gl_obj->setName("score_phase1"); gl_obj->setLineWidth(1.0f); gl_obj->setVerticesPointSize(2.0f); gl_obj->setColor_u8(mrpt::img::TColor(0xff, 0xff, 0x00, 0xff)); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CSetOfLines::Create(); gl_obj->setName("score_phase2"); gl_obj->setLineWidth(1.0f); gl_obj->setVerticesPointSize(2.0f); gl_obj->setColor_u8(mrpt::img::TColor(0xff, 0xff, 0xff, 0xff)); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CSetOfLines::Create(); gl_obj->setName("tp_selected_dir"); gl_obj->setLineWidth(3.0f); gl_obj->setColor_u8(mrpt::img::TColor(0x00, 0xff, 0x00, 0xd0)); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CSetOfObjects::Create(); gl_obj->setName("tp_target"); gl_obj->setLocation(0, 0, 0.02f); scene->insert(gl_obj); } { auto gl_obj = mrpt::opengl::CPointCloud::Create(); gl_obj->setName("tp_robot"); gl_obj->setPointSize(4.0f); gl_obj->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00, 0xa0)); gl_obj->setLocation(0, 0, 0.02f); scene->insert(gl_obj); } #if 0 // should be re-done with CMesh3D? { auto gl_obj = mrpt::opengl::CMesh::Create(true /*transparency*/); gl_obj->setName("tp_clearance"); gl_obj->setScale(1.0f, 1.0f, 5.0f); scene->insert(gl_obj); } #endif } mrpt::opengl::Scene::Ptr& scene = viz.glCanvas->scene; auto view = scene->getViewport(); // Draw dynamic stuff: const CLogFileRecord::TInfoPerPTG& pI = log.infoPerPTG[nPTG]; vector xs, ys; const size_t nAlphas = pI.TP_Obstacles.size(); view->clearTextMessages(); auto fp = getFontParams(); fp.color = is_selected_ptg ? TColorf(1.0f, 1.0f, 0.f) : TColorf(1.0f, 1.0f, 1.0f); view->addTextMessage( 4, 4, mrpt::format("[%u]:%s", nPTG, log.infoPerPTG[nPTG].PTG_desc.c_str()), 0 /*id*/, fp); // Chosen direction: { const double aDir = pI.desiredDirection; auto gl_obj = std::dynamic_pointer_cast(scene->getByName("tp_selected_dir")); gl_obj->clear(); gl_obj->appendLine(0, 0, 0, pI.desiredSpeed * cos(aDir), pI.desiredSpeed * sin(aDir), 0); } // obstacles: xs.clear(); ys.clear(); xs.reserve(nAlphas); ys.reserve(nAlphas); for (size_t i = 0; i < nAlphas; ++i) { const double a = -M_PI + (i + 0.5) * 2 * M_PI / double(nAlphas); const double r = pI.TP_Obstacles[i]; xs.push_back(r * cos(a)); ys.push_back(r * sin(a)); } { auto gl_obj = std::dynamic_pointer_cast(scene->getByName("tp_obstacles")); gl_obj->clear(); if (nAlphas > 2) { gl_obj->appendLine(xs[0], ys[0], 0, xs[1], ys[1], 0); for (size_t i = 2; i < nAlphas; i++) gl_obj->appendLineStrip(xs[i], ys[i], 0); } } // Target: { auto gl_obj = std::dynamic_pointer_cast(scene->getByName("tp_target")); gl_obj->clear(); auto glTargetPts = mrpt::opengl::CPointCloud::Create(); glTargetPts->setPointSize(5); glTargetPts->setColor_u8(0, 0, 0); gl_obj->insert(glTargetPts); for (const auto& p : pI.TP_Targets) { auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.15f); glCorner->setPose(p); gl_obj->insert(glCorner); glTargetPts->insertPoint(p.x, p.y, 0.02); } if (!pI.TP_Targets.empty()) { double ang = ::atan2(pI.TP_Targets[0].y, pI.TP_Targets[0].x); int tp_target_k = 0; if (sel_ptg_idx < int(m_logdata_ptg_paths.size()) && sel_ptg_idx >= 0) { mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg = m_logdata_ptg_paths[sel_ptg_idx]; if (ptg) { tp_target_k = ptg->alpha2index(ang); } } auto fp2 = getFontParams(); fp2.draw_shadow = false; view->addTextMessage( 4, -12, mrpt::format( "TP_Target[0]=(%.02f,%.02f) k=%i ang=%.02f deg", pI.TP_Targets[0].x, pI.TP_Targets[0].y, tp_target_k, mrpt::RAD2DEG(ang)), 1 /*id*/, fp2); } } if (m_cbShowAllDebugFields->checked()) { unsigned int ptg_unique_id = 2; int ptg_lineY = 1; for (const auto& e : pI.evalFactors) { view->addTextMessage( 4, 5 + (ptg_lineY++) * (getFontParams().vfont_scale + 3), mrpt::format("%20s=%6.03f", e.first.c_str(), e.second), ptg_unique_id++, getFontParams()); } } // Current robot pt (normally in pure reactive, at (0,0)): { auto gl_obj = std::dynamic_pointer_cast(scene->getByName("tp_robot")); gl_obj->clear(); gl_obj->insertPoint(pI.TP_Robot.x, pI.TP_Robot.y, 0); } // Clearance-diagram: #if 0 { auto gl_obj = std::dynamic_pointer_cast( scene->getByName("tp_clearance")); if (pI.clearance.empty()) gl_obj->setVisibility(false); else { gl_obj->setVisibility(true); pI.clearance.renderAs3DObject( *gl_obj, -1.0, 1.0, -1.0, 1.0, 0.15, m_ClearanceOverPath->checked() /*interp over path*/); } } #endif // Scores { auto gl_obj1 = std::dynamic_pointer_cast(scene->getByName("score_phase1")); auto gl_obj2 = std::dynamic_pointer_cast(scene->getByName("score_phase2")); const bool visible1 = m_rbPerPTGPlots->selectedIndex() >= 1; const bool visible2 = m_rbPerPTGPlots->selectedIndex() >= 2; gl_obj1->clear(); gl_obj2->clear(); gl_obj1->setVisibility(visible1); gl_obj2->setVisibility(visible2); if ((visible1 || visible2) && pI.HLFR && nAlphas > 2) { const auto& dir_evals = pI.HLFR->dirs_eval; const bool has_scores = !dir_evals.empty() && dir_evals[0].size() == nAlphas; const size_t num_scores = dir_evals.size(); for (size_t iScore = 0; has_scores && iScore < num_scores; iScore++) { vector pts; pts.reserve(nAlphas); for (size_t i = 0; i < nAlphas; ++i) { const double a = -M_PI + (i + 0.5) * 2 * M_PI / double(nAlphas); const double r = dir_evals[iScore][i]; pts.emplace_back(r * cos(a), r * sin(a)); } mrpt::opengl::CSetOfLines::Ptr& gl_obj = iScore == (num_scores - 1) ? gl_obj1 : gl_obj2; gl_obj->appendLine(pts[0].x, pts[0].y, 0, pts[1].x, pts[1].y, 0); for (size_t i = 2; i < nAlphas; i++) gl_obj->appendLineStrip(pts[i].x, pts[i].y, 0); } } } } // end for each PTG if (mustPerformLayout) m_win->performLayout(); NANOGUI_END_TRY(*m_win) } // ------------------------------------------------------------------------ // Generate a MATLAB script that draws the overall navigation log // ------------------------------------------------------------------------ void NavlogViewerApp::OnmnuMatlabPlotsSelected() { NANOGUI_START_TRY const std::string fileName = nanogui::file_dialog( { {"m", "MATLAB/Octave script"} }, true /*save*/); if (fileName.empty()) return; ofstream f(fileName); if (!f.is_open()) throw runtime_error("Error writing to file!"); f << "% Script for drawing navigation log\n" << "% Generated automatically by navlog-viewer - MRPT " << mrpt::system::MRPT_getVersion() << "\n" << "% From log: " << m_openedFileName << "\n" << "% " "-------------------------------------------------------------" "--------" "----\n\n"; f << "%%\n" << "function [] = main()\n" << "figure;" << "title('Path for " << mrpt::system::extractFileName(fileName) << "');" << "% Robot shape: (x,y) in each line\n" << "rs = [-0.3 -0.3;0.6 -0.3;0.6 0.3;-0.3 0.3];\n" << "dec_shps = 15;" << "dec=0;"; const int DECIMATE_POINTS = 10; int decim_point_cnt = 0; std::vector X, Y; // Obstacles std::vector TX, TY; // Target over time const size_t N = m_logdata.size(); for (size_t i = 0; i < N; i++) { const CLogFileRecord::Ptr logsptr = std::dynamic_pointer_cast(m_logdata[i]); const CLogFileRecord* logptr = logsptr.get(); const auto robotPose = logptr->robotPoseLocalization; CPose2D observationBasePose = CPose2D(robotPose); if (m_cbShowDelays->checked()) observationBasePose = observationBasePose + CPose2D(logptr->relPoseSense); f << mrpt::format( "dec=dec+1; if (dec>=dec_shps); drawRobotShape(rs,[%f %f " "%f]); " "dec=0; end\n", robotPose.x, robotPose.y, robotPose.phi); if (++decim_point_cnt >= DECIMATE_POINTS) { CSimplePointsMap pts; pts.changeCoordinatesReference(logptr->WS_Obstacles, CPose3D(observationBasePose)); const auto& pX = pts.getPointsBufferRef_x(); const auto& pY = pts.getPointsBufferRef_y(); X.insert(X.begin(), pX.begin(), pX.end()); Y.insert(Y.begin(), pY.begin(), pY.end()); } // Target: const mrpt::math::TPoint2D trg_glob = mrpt::math::TPoint2D(robotPose + logptr->WS_targets_relative[0]); if (TX.empty() || std::abs((*TX.rbegin()) - trg_glob.x) > 1e-3 || std::abs((*TY.rbegin()) - trg_glob.y) > 1e-3) { TX.push_back(trg_glob.x); TY.push_back(trg_glob.y); } } f << "\n % Points: \n" << " Ps = ["; for (size_t k = 0; k < X.size(); k++) { f << X[k] << " " << Y[k] << "\n"; } f << "];\n" << "plot(Ps(:,1),Ps(:,2),'k.','MarkerSize',3);\n"; f << "\n % Target point: \n" << " Ts = ["; for (size_t k = 0; k < TX.size(); k++) { f << TX[k] << " " << TY[k] << "\n"; } f << "];\n" << "plot(Ts(:,1),Ts(:,2),'rx','MarkerSize',10);\n"; f << "axis equal;\n" << "\n"; f << "%% drawRobotShape()\n" << "function [] = drawRobotShape(sh,pose)\n" << "nPts=size(sh,1);\n" << "Pts=[];\n" << "for i=1:(nPts+1),\n" << " j=mod(i-1,nPts)+1;\n" << " cc=cos(pose(3)); ss=sin(pose(3)); x=pose(1); y=pose(2);\n" << " Pts=[Pts;x+cc*sh(j,1)-ss*sh(j,2) y+ss*sh(j,1)+cc*sh(j,2) " "];\n" << "end\n" << "plot(Pts(:,1),Pts(:,2)); hold on;\n"; NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnmnuSeePTGParamsSelected() { NANOGUI_START_TRY const std::string sSection = "PTG_PARAMS"; mrpt::config::CConfigFileMemory cfg; cfg.write(sSection, "PTG_COUNT", m_logdata_ptg_paths.size()); CConfigFilePrefixer cfg_pre; cfg_pre.bind(cfg); for (size_t i = 0; i < m_logdata_ptg_paths.size(); i++) { mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg = m_logdata_ptg_paths[i]; if (!ptg) continue; const std::string sKeyPrefix = mrpt::format("PTG%d_", (int)i); cfg_pre.setPrefixes("", sKeyPrefix); ptg->saveToConfigFile(cfg_pre, sSection); } const std::string sCfgText = cfg.getContent(); const std::string fileName = nanogui::file_dialog( { {"ini", "Save PTG configuration"} }, true /*save*/); if (fileName.empty()) return; std::ofstream f(fileName); f << sCfgText; NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnmnuGenerateOdometryVsLocalizationReport() { NANOGUI_START_TRY const std::string fileName = nanogui::file_dialog( { {"txt", "Save localization vs odometry report"} }, true /*save*/); if (fileName.empty()) return; std::ofstream f(fileName); ASSERT_(f.is_open()); f << "% timestamp localizationPoseIncr odometryPoseIncr\n"; std::optional lastOdo, lastLocPose; const size_t N = m_logdata.size(); for (size_t i = 0; i < N; i++) { const CLogFileRecord::Ptr logsptr = std::dynamic_pointer_cast(m_logdata[i]); const CLogFileRecord* logptr = logsptr.get(); if (!logptr) continue; const auto& log = *logptr; const auto& odo = log.robotPoseOdometry; const auto& pose = log.robotPoseLocalization; if (lastOdo && lastLocPose) { const auto odoIncr = odo - *lastOdo; const auto locIncr = pose - *lastLocPose; if (!log.timestamps.empty()) f << mrpt::format("%.03f ", mrpt::Clock::toDouble(log.timestamps.begin()->second)); else f << "0 "; for (int k = 0; k < 3; k++) f << locIncr[k] << " "; for (int k = 0; k < 3; k++) f << odoIncr[k] << " "; f << "\n"; } lastOdo = odo; lastLocPose = pose; } NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnmnuSaveScoreMatrixSelected() { NANOGUI_START_TRY const std::string fileName = nanogui::file_dialog( { {"txt", "Save score matrices"} }, true /*save*/); if (fileName.empty()) return; const size_t N = m_logdata.size(); for (size_t i = 0; i < N; i++) { const CLogFileRecord::Ptr logsptr = std::dynamic_pointer_cast(m_logdata[i]); const CLogFileRecord* logptr = logsptr.get(); for (size_t iPTG = 0; iPTG < logptr->infoPerPTG.size(); iPTG++) { const CHolonomicLogFileRecord::Ptr& hlog = logptr->infoPerPTG[iPTG].HLFR; if (!hlog) continue; const mrpt::math::CMatrixD* dirs_scores = hlog->getDirectionScores(); if (!dirs_scores || dirs_scores->rows() < 2) continue; const std::string sFil = mrpt::system::fileNameChangeExtension( fileName, mrpt::format("step%06u_ptg%02u.txt", (unsigned int)i, (unsigned int)iPTG)); dirs_scores->saveToTextFile(sFil, mrpt::math::MATRIX_FORMAT_FIXED); } } NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnmnuSaveCurrentObstacles() { NANOGUI_START_TRY const int log_idx = mrpt::round(slidLog->value()); if (log_idx >= int(m_logdata.size())) return; auto logptr = std::dynamic_pointer_cast(m_logdata[log_idx]); const CLogFileRecord& log = *logptr; const std::string fileName = nanogui::file_dialog( { {"txt", "Obstacles ASCII plain text file"} }, true /*save*/); if (fileName.empty()) return; log.WS_Obstacles_original.save2D_to_text_file(fileName); NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OntimMouseXY() { // Mouse position at Z=0 if (!m_win) return; const auto mousexY = m_win->mousePos(); mrpt::math::TLine3D mouse_ray; m_win->background_scene->getViewport("main")->get3DRayForPixelCoord( mousexY.x(), mousexY.y(), mouse_ray); int lineY = 0, unique_id = 0; // Create a 3D plane, e.g. Z=0 const mrpt::math::TPlane ground_plane(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0)); // Intersection of the line with the plane: mrpt::math::TObject3D inters; mrpt::math::intersect(mouse_ray, ground_plane, inters); // Interpret the intersection as a point, if there is an // intersection: mrpt::math::TPoint3D inters_pt; if (inters.getPoint(inters_pt)) { ADD_WIN_TEXTMSG(mrpt::format("Mouse pos: X=%.04f Y=%.04f", inters_pt.x, inters_pt.y)); } } void NavlogViewerApp::OnmnuExportSelected(std::string filename) { NANOGUI_START_TRY; const int log_idx = mrpt::round(slidLog->value()); if (log_idx >= int(m_logdata.size())) return; auto logptr = std::dynamic_pointer_cast(m_logdata[log_idx]); const CLogFileRecord& log = *logptr; if (filename.empty()) filename = nanogui::file_dialog( { {"navlog", "MRPT navlog file"} }, true /*save*/); if (filename.empty()) return; mrpt::io::CCompressedOutputStream f(filename); ASSERT_(f.is_open()); auto a = mrpt::serialization::archiveFrom(f); a << log; NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnmnuMatlabExportPaths() { NANOGUI_START_TRY; const size_t N = m_logdata.size(); ASSERTMSG_(N > 0, "Log is empty! Load a valid log first..."); const std::string fileName = nanogui::file_dialog( { {"m", "MATLAB/Octave script"} }, true /*save*/); if (fileName.empty()) return; ofstream f(fileName); if (!f.is_open()) throw runtime_error("Error writing to file!"); f << "% Script for drawing navigation paths\n" << "% Generated automatically by navlog-viewer - MRPT " << mrpt::system::MRPT_getVersion() << "\n" << "% From log: " << m_openedFileName << "\n" << "% " "-------------------------------------------------------------" "----" "----" "----\n\n"; std::map selected_PTG_over_time; // time: tim_start_iteration std::map iteration_duration; // time: tim_start_iteration struct TRobotPoseVel { mrpt::math::TPose2D pose, poseOdom; mrpt::math::TTwist2D velGlobal, velLocal; }; std::map global_local_vel; // time: curPoseAndVel std::map vals_timoff_obstacles, vals_timoff_curPoseVelAge, vals_timoff_sendVelCmd; // time: tim_start_iteration const int MAX_CMDVEL_COMPONENTS = 15; using cmdvel_vector_t = mrpt::math::CVectorDouble; // map: time -> tim_send_cmd_vel std::map cmdvels; double tim_start_iteration = .0; for (size_t i = 0; i < N; i++) { const CLogFileRecord::Ptr logsptr = std::dynamic_pointer_cast(m_logdata[i]); const CLogFileRecord* logptr = logsptr.get(); { const auto it = logptr->timestamps.find("tim_start_iteration"); if (it != logptr->timestamps.end()) { tim_start_iteration = mrpt::Clock::toDouble(it->second); } else { tim_start_iteration++; // just in case we don't have // valid // iteration timestamp (??) } } // selected PTG: selected_PTG_over_time[tim_start_iteration] = logptr->nSelectedPTG; // iter dur: { auto itExecT = logptr->values.find("executionTime"); if (itExecT != logptr->values.end()) iteration_duration[tim_start_iteration] = itExecT->second; } // timoff_obstacles; { const auto it = logptr->values.find("timoff_obstacles"); if (it != logptr->values.end()) vals_timoff_obstacles[tim_start_iteration] = it->second; } // timoff_curPoseVelAge; { const auto it = logptr->values.find("timoff_curPoseVelAge"); if (it != logptr->values.end()) vals_timoff_curPoseVelAge[tim_start_iteration] = it->second; } // timoff_sendVelCmd { const auto it = logptr->values.find("timoff_sendVelCmd"); if (it != logptr->values.end()) vals_timoff_sendVelCmd[tim_start_iteration] = it->second; } // curPoseAndVel: { double tim_pose = tim_start_iteration; // default const auto it = logptr->timestamps.find("curPoseAndVel"); if (it != logptr->timestamps.end()) { tim_pose = mrpt::Clock::toDouble(it->second); } auto& p = global_local_vel[tim_pose]; p.pose = logptr->robotPoseLocalization; p.poseOdom = logptr->robotPoseOdometry; p.velGlobal = logptr->cur_vel; p.velLocal = logptr->cur_vel_local; } // send cmd vels: if (logptr->cmd_vel) { double tim_send_cmd_vel = tim_start_iteration; // default const auto it = logptr->timestamps.find("tim_send_cmd_vel"); if (it != logptr->timestamps.end()) { tim_send_cmd_vel = mrpt::Clock::toDouble(it->second); } auto& p = cmdvels[tim_send_cmd_vel]; p.resize(MAX_CMDVEL_COMPONENTS); p.setZero(); for (size_t k = 0; k < logptr->cmd_vel->getVelCmdLength(); k++) p[k] = logptr->cmd_vel->getVelCmdElement(k); } } // end for each timestep double t_ref = 0; if (!selected_PTG_over_time.empty()) { t_ref = selected_PTG_over_time.begin()->first; } f << "clear; close all;\n"; f << "% robot pose over time. Columns: [time curPoseAndVel, " "x,y,phi_rad, " "odo_x,odo_y,odo_phi_rad]\n" "robot_pose = ["; for (const auto& e : global_local_vel) { f << (e.first - t_ref) << "," << e.second.pose.x << "," << e.second.pose.y << "," << e.second.pose.phi << ", " << e.second.poseOdom.x << "," << e.second.poseOdom.y << "," << e.second.poseOdom.phi << " ; "; } f << "];\n" "robot_pose(:,4) = unwrap(robot_pose(:,4));\n" "figure(); subplot(2,1,1); \n" "plot(robot_pose(:, 1), robot_pose(:, 2 : 4), '.', " "robot_pose(:, " "1), " "robot_pose(:, 2 : 4), '-'); xlabel('Time'); legend('x', 'y', " "'phi " "(rad)'); title('robot pose'); \n" "xl=xlim; subplot(2,1,2);\n" "robot_pose_diff = diff(robot_pose(:, 2 : 4));\n" "idxs_rob_incr_z = find(robot_pose_diff(:, 1) == 0 & " "robot_pose_diff(:, 2) == 0 & robot_pose_diff(:, 3) == 0);\n" "plot(robot_pose(2:end, 1), diff(robot_pose(:, 2 : 4)), " "'.');\n" "hold on;\n" "plot(robot_pose(idxs_rob_incr_z, 1), " "robot_pose_diff(idxs_rob_incr_z, :), 'k', 'MarkerSize', 11, " "'Marker', 'square');\n" "xlabel('Time'); legend('x', 'y', 'phi (rad)'); title('Robot " "pose " "*increments*');\n" "xlim(xl);\n\n"; f << "% Selected PTG over time. Columns: [tim_start_iteration, " "0-based " "index selected PTG]\n" "selected_PTG = ["; for (const auto& e : selected_PTG_over_time) { f << (e.first - t_ref) << "," << e.second << " ; "; } f << "];\n" "figure(); plot(selected_PTG(:,1),selected_PTG(:,2), 'x'); " "xlabel('Time'); ylabel('Selected PTG');\n\n"; if (!iteration_duration.empty()) { f << "% Iteration duration. Columns: [tim_start_iteration, " "iter_duration_seconds]\n" "iteration_duration = ["; for (const auto& e : iteration_duration) { f << (e.first - t_ref) << "," << e.second << " ; "; } f << "];\n" "figure(); " "plot(iteration_duration(:,1),iteration_duration(:,2), " "'x');\n" "hold on;\n" "plot(selected_PTG(1:(end-1),1),diff(selected_PTG(:,1)),'." "')" ";" "xlabel('Time'); legend('Iteration duration', 'Diff " "consecutive " "call time'); title('rnav_iter_call_time_duration');\n\n"; } if (!vals_timoff_obstacles.empty()) { f << "% vals_timoff_obstacles. Columns: [tim_start_iteration, " "value]\n" "timoff_obstacles = ["; for (const auto& e : vals_timoff_obstacles) { f << (e.first - t_ref) << " , " << e.second << " ; "; } f << "];\n" "figure(); " "plot(timoff_obstacles(:,1),timoff_obstacles(:,2), " "'.',timoff_obstacles(:,1),timoff_obstacles(:,2), '-'); " "xlabel('Time'); title('timoff_obstacles');\n\n"; } if (!vals_timoff_curPoseVelAge.empty()) { f << "% vals_timoff_curPoseVelAge. Columns: " "[tim_start_iteration, " "value]\n" "timoff_curPoseVelAge = ["; for (const auto& e : vals_timoff_curPoseVelAge) { f << (e.first - t_ref) << " , " << e.second << " ; "; } f << "];\n" "figure(); " "plot(timoff_curPoseVelAge(:,1),timoff_curPoseVelAge(:,2)," " " "'.',timoff_curPoseVelAge(:,1),timoff_curPoseVelAge(:,2), " "'-'); " "xlabel('Time'); title('timoff_curPoseVelAge');\n\n"; } if (!vals_timoff_sendVelCmd.empty()) { f << "% vals_timoff_sendVelCmd. Columns: [tim_start_iteration, " "value]\n" "timoff_sendVelCmd = ["; for (const auto& e : vals_timoff_sendVelCmd) { f << (e.first - t_ref) << " , " << e.second << " ; "; } f << "];\n" "figure(); plot(timoff_sendVelCmd (:,1),timoff_sendVelCmd " "(:,2), " "'.',timoff_sendVelCmd (:,1),timoff_sendVelCmd (:,2), " "'-'); " "xlabel('Time'); title('timoff_sendVelCmd ');\n\n"; } f << "% robot vel over time. Columns: [time curPoseAndVel, " "vx,vy,omega_rad_sec]\n" "robot_vel_global = ["; for (const auto& e : global_local_vel) { f << (e.first - t_ref) << "," << e.second.velGlobal.vx << "," << e.second.velGlobal.vy << "," << e.second.velGlobal.omega << " ; "; } f << "];\n" "figure(); " "plot(robot_vel_global(:,1),robot_vel_global(:,2:4), " "'.', " "robot_vel_global(:,1),robot_vel_global(:,2:4), '-'); " "xlabel('Time'); " "title('Velocities (global)'); legend('vx','vy','omega');\n\n"; f << "robot_vel_local = ["; for (const auto& e : global_local_vel) { f << (e.first - t_ref) << "," << e.second.velLocal.vx << "," << e.second.velLocal.vy << "," << e.second.velLocal.omega << " ; "; } f << "];\n" "figure(); plot(robot_vel_local(:,1),robot_vel_local(:,2:4), " "'.', " "robot_vel_local(:,1),robot_vel_local(:,2:4), '-'); " "xlabel('Time'); " "title('Velocities (local)'); legend('vx','vy','omega');\n\n"; // cmdvels: f << "% Movement commands sent to robot. Columns: [time " "curPoseAndVel, " "vx,vy,omega_rad_sec]\n" "cmdvels = ["; for (const auto& e : cmdvels) { f << (e.first - t_ref) << " "; f << e.second << " ; "; } f << "];\n" "figure(); plot(cmdvels(:,1),cmdvels(:,2:" << (MAX_CMDVEL_COMPONENTS + 1) << "), '.', cmdvels(:,1),cmdvels(:,2:" << (MAX_CMDVEL_COMPONENTS + 1) << "), '-'); xlabel('Time'); title('Issued motion commands " "(meaning " "CVehicleVelCmd-dependend)');\n\n"; NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::OnManualPickAppendYaml() { NANOGUI_START_TRY; const std::string yamlFileName = "nav-selected.yaml"; mrpt::containers::yaml d; if (mrpt::system::fileExists(yamlFileName)) { // load for appending: d = mrpt::containers::yaml::FromFile(yamlFileName); } else { // Start from scratch: d = mrpt::containers::yaml::Sequence(); } // Export individual navlog file: const std::string navLogSingleFile = mrpt::format("log_%.03f.navlog", m_manualPickTimestamp); OnmnuExportSelected(navLogSingleFile); // Append entry: mrpt::containers::yaml entry = mrpt::containers::yaml::Map(); entry["file"] = navLogSingleFile; entry["entry_index"] = 0; // since we are exporting a single-entry file entry["selected_ptg_index"] = m_manualPickPTGIdx; entry["selected_trajectory_index"] = m_manualPickTrajectoryIdx; d.asSequence().push_back(entry); std::ofstream fOut(yamlFileName); d.printAsYAML(fOut); NANOGUI_END_TRY(*m_win) } void NavlogViewerApp::updateManualPickControls() { NANOGUI_START_TRY; m_manualPickPTGIdx = -1; m_manualPickTrajectoryIdx = -1; const bool manualPickEnabled = m_cbManualPickMode->checked(); m_slidPtgIndex->setEnabled(manualPickEnabled); m_slidTrajectoryIndex->setEnabled(manualPickEnabled); m_btnManualPickAppendYaml->setEnabled(manualPickEnabled); const int log_idx = mrpt::round(slidLog->value()); if (log_idx >= int(m_logdata.size())) return; auto logptr = std::dynamic_pointer_cast(m_logdata[log_idx]); const CLogFileRecord& log = *logptr; m_slidPtgIndex->setRange({0, log.nPTGs - 1}); const int manualPickedPTG = mrpt::round(m_slidPtgIndex->value()); const auto& ptg = m_logdata_ptg_paths.at(manualPickedPTG); ASSERT_(ptg); m_slidTrajectoryIndex->setRange({0, ptg->getPathCount() - 1}); const int manualPickedTraj = mrpt::round(m_slidTrajectoryIndex->value()); m_manualPickLabel->setCaption( mrpt::format("Selected PTG#%i trajectory #%i", manualPickedPTG, manualPickedTraj)); if (manualPickEnabled) { if (!log.timestamps.empty()) m_manualPickTimestamp = mrpt::Clock::toDouble(log.timestamps.begin()->second); m_manualPickPTGIdx = manualPickedPTG; m_manualPickTrajectoryIdx = manualPickedTraj; } NANOGUI_END_TRY(*m_win) }