10 #include <mrpt/core/format.h> 11 #include <mrpt/core/lock_helper.h> 12 #include <mrpt/core/round.h> 13 #include <mrpt/math/TLine3D.h> 14 #include <mrpt/math/TObject3D.h> 15 #include <mrpt/math/geometry.h> 19 #include <mrpt/version.h> 25 #if MRPT_VERSION >= 0x204 26 #include <mrpt/system/thread_name.h> 29 using namespace mvsim;
49 #if MRPT_VERSION >= 0x211 50 nanogui::Window* winMenu = gui_win->createManagedSubWindow(
"Control");
52 nanogui::Window* winMenu =
new nanogui::Window(gui_win.get(),
"Control");
56 gui_win->getSubWindowsUI()->setPosition({1, 1});
58 winMenu->setPosition({1, 80});
59 winMenu->setLayout(
new nanogui::BoxLayout(
60 nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 5));
62 winMenu->add<nanogui::Button>(
"Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
63 ->setCallback([
this]() {
64 gui_win->setVisible(
false);
69 ->add<nanogui::CheckBox>(
71 [&](
bool b) { gui_win->camera().setCameraProjective(!
b); })
72 ->setChecked(m_parent.m_gui_options.ortho);
74 std::vector<std::string> lstVehicles;
75 lstVehicles.reserve(m_parent.m_vehicles.size() + 1);
77 lstVehicles.push_back(
"[none]");
78 for (
const auto&
v : m_parent.m_vehicles) lstVehicles.push_back(
v.first);
80 winMenu->add<nanogui::Label>(
"Camera follows:");
81 auto cbFollowVeh = winMenu->add<nanogui::ComboBox>(lstVehicles);
82 cbFollowVeh->setSelectedIndex(0);
83 cbFollowVeh->setCallback([
this, lstVehicles](
int idx) {
85 m_parent.m_gui_options.follow_vehicle.clear();
86 else if (idx <= static_cast<int>(m_parent.m_vehicles.size()))
87 m_parent.m_gui_options.follow_vehicle = lstVehicles[idx];
94 #if MRPT_VERSION >= 0x211 95 nanogui::Window*
w = gui_win->createManagedSubWindow(
"Status");
97 nanogui::Window* w =
new nanogui::Window(gui_win.get(),
"Status");
100 w->setPosition({140, 80});
101 w->setLayout(
new nanogui::BoxLayout(
102 nanogui::Orientation::Vertical, nanogui::Alignment::Fill));
103 w->setFixedWidth(270);
105 #if MRPT_VERSION < 0x211 107 ->add<nanogui::Button>(
"", ENTYPO_ICON_CROSS)
108 ->setCallback([w]() { w->setVisible(
false); });
111 lbCpuUsage = w->add<nanogui::Label>(
" ");
112 lbStatuses.resize(5);
113 for (
size_t i = 0; i < lbStatuses.size(); i++)
114 lbStatuses[i] = w->add<nanogui::Label>(
" ");
120 #if MRPT_VERSION >= 0x211 121 #if MRPT_VERSION >= 0x231 122 const auto subwinIdx = gui_win->getSubwindowCount();
124 nanogui::Window*
w = gui_win->createManagedSubWindow(
"Editor");
126 nanogui::Window* w =
new nanogui::Window(gui_win.get(),
"Editor");
129 const int pnWidth = 250, pnHeight = 200;
131 w->setPosition({1, 230});
132 w->setLayout(
new nanogui::BoxLayout(
133 nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 3, 3));
134 w->setFixedWidth(pnWidth);
136 #if MRPT_VERSION < 0x211 138 ->add<nanogui::Button>(
"", ENTYPO_ICON_CROSS)
139 ->setCallback([w]() { w->setVisible(
false); });
142 w->add<nanogui::Label>(
"Selected object",
"sans-bold");
144 if (!m_parent.m_simulableObjects.empty())
146 auto tab = w->add<nanogui::TabWidget>();
148 nanogui::Widget* tabs[3] = {
149 tab->createTab(
"Vehicles"), tab->createTab(
"Blocks"),
150 tab->createTab(
"Elements")};
152 tab->setActiveTab(0);
155 t->setLayout(
new nanogui::BoxLayout(
156 nanogui::Orientation::Vertical, nanogui::Alignment::Minimum, 3,
159 nanogui::VScrollPanel* vscrolls[3] = {
160 tabs[0]->add<nanogui::VScrollPanel>(),
161 tabs[1]->add<nanogui::VScrollPanel>(),
162 tabs[2]->add<nanogui::VScrollPanel>()};
164 for (
auto vs : vscrolls) vs->setFixedSize({pnWidth, pnHeight});
168 nanogui::Widget* wrappers[3];
169 for (
int i = 0; i < 3; i++)
171 wrappers[i] = vscrolls[i]->add<nanogui::Widget>();
172 wrappers[i]->setFixedSize({pnWidth, pnHeight});
173 wrappers[i]->setLayout(
new nanogui::GridLayout(
174 nanogui::Orientation::Horizontal, 1 ,
175 nanogui::Alignment::Minimum, 3, 3));
178 for (
const auto& o : m_parent.m_simulableObjects)
182 const auto&
name = o.first;
183 bool isVehicle =
false;
184 if (
auto v = dynamic_cast<VehicleBase*>(o.second.get());
v)
189 bool isBlock =
false;
190 if (
auto v = dynamic_cast<Block*>(o.second.get());
v)
195 bool isWorldElement =
false;
196 if (
auto v = dynamic_cast<WorldElementBase*>(o.second.get());
v)
198 isWorldElement =
true;
202 isVehicle ? wrappers[0] : (isBlock ? wrappers[1] : wrappers[2]);
204 std::string label =
name;
205 if (label.empty()) label =
"(unnamed)";
207 auto cb = wrapper->add<nanogui::CheckBox>(label);
210 gui_cbObjects.emplace_back(ipo);
212 cb->setChecked(
false);
213 cb->setCallback([cb, ipo,
this](
bool check) {
215 if (gui_selectedObject.visual)
216 gui_selectedObject.visual->showBoundingBox(
false);
217 if (gui_selectedObject.cb)
218 gui_selectedObject.cb->setChecked(
false);
221 cb->setChecked(check);
226 gui_selectedObject = ipo;
230 const bool btnsEnabled = !!gui_selectedObject.simulable;
231 for (
auto b : btns_selectedOps)
b->setEnabled(btnsEnabled);
236 w->add<nanogui::Label>(
" ");
238 btnReplaceObject = w->add<nanogui::Button>(
"Click to replace...");
239 btnReplaceObject->setFlags(nanogui::Button::Flags::ToggleButton);
240 btns_selectedOps.push_back(btnReplaceObject);
243 auto pn = w->add<nanogui::Widget>();
244 pn->setLayout(
new nanogui::BoxLayout(
245 nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 2, 2));
246 pn->add<nanogui::Label>(
"Reorient:");
247 auto slAngle = pn->add<nanogui::Slider>();
249 slAngle->setCallback([
this](
float v) {
250 if (!gui_selectedObject.simulable)
return;
251 auto p = gui_selectedObject.simulable->getPose();
253 gui_selectedObject.simulable->setPose(
p);
255 slAngle->setFixedWidth(150);
256 btns_selectedOps.push_back(slAngle);
259 auto btnPlaceCoords = w->add<nanogui::Button>(
"Replace by coordinates...");
260 btns_selectedOps.push_back(btnPlaceCoords);
261 btnPlaceCoords->setCallback([
this]() {
263 if (!gui_selectedObject.simulable)
return;
265 auto* formPose =
new nanogui::Window(gui_win.get(),
"Enter new pose");
266 formPose->setLayout(
new nanogui::GridLayout(
267 nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5));
269 nanogui::TextBox* lbs[3];
271 formPose->add<nanogui::Label>(
"x coordinate:");
272 lbs[0] = formPose->add<nanogui::TextBox>();
273 formPose->add<nanogui::Label>(
"y coordinate:");
274 lbs[1] = formPose->add<nanogui::TextBox>();
275 formPose->add<nanogui::Label>(
"Orientation:");
276 lbs[2] = formPose->add<nanogui::TextBox>();
278 for (
int i = 0; i < 3; i++)
280 lbs[i]->setEditable(
true);
281 lbs[i]->setFixedSize({100, 20});
282 lbs[i]->setValue(
"0.0");
283 lbs[i]->setUnits(i == 2 ?
"[deg]" :
"[m]");
284 lbs[i]->setDefaultValue(
"0.0");
285 lbs[i]->setFontSize(16);
286 lbs[i]->setFormat(
"[-]?[0-9]*\\.?[0-9]+");
289 const auto pos = gui_selectedObject.simulable->getPose();
290 lbs[0]->setValue(std::to_string(pos.x));
291 lbs[1]->setValue(std::to_string(pos.y));
292 lbs[2]->setValue(std::to_string(mrpt::RAD2DEG(pos.yaw)));
294 formPose->add<nanogui::Label>(
"");
295 formPose->add<nanogui::Label>(
"");
297 formPose->add<nanogui::Button>(
"Cancel")->setCallback(
298 [formPose]() { formPose->dispose(); });
300 formPose->add<nanogui::Button>(
"Accept")->setCallback(
301 [formPose,
this, lbs]() {
302 gui_selectedObject.simulable->setPose(
304 std::stod(lbs[0]->
value()),
306 std::stod(lbs[1]->
value()),
310 mrpt::DEG2RAD(std::stod(lbs[2]->
value())),
318 formPose->setModal(
true);
320 formPose->setVisible(
true);
323 for (
auto b : btns_selectedOps)
b->setEnabled(
false);
326 #if MRPT_VERSION >= 0x231 327 gui_win->subwindowMinimize(subwinIdx);
329 if (
auto btnMinimize =
330 dynamic_cast<nanogui::Button*>(w->buttonPanel()->children().at(0));
333 btnMinimize->callback()();
348 mrpt::gui::CDisplayWindowGUI_Params cp;
349 cp.maximized = m_gui_options.start_maximized;
351 m_gui.gui_win = mrpt::gui::CDisplayWindowGUI::Create(
352 "mvsim", m_gui_options.win_w, m_gui_options.win_h, cp);
355 auto scene = mrpt::opengl::COpenGLScene::Create();
357 std::lock_guard<std::mutex> lck(
358 m_gui.gui_win->background_scene_mtx);
359 m_gui.gui_win->background_scene = std::move(
scene);
363 if (m_world_elements.empty())
366 m_world_elements.push_back(we);
370 m_gui.prepare_top_menu();
371 m_gui.prepare_status_window();
372 m_gui.prepare_editor_window();
375 m_gui.gui_win->performLayout();
376 auto& cam = m_gui.gui_win->camera();
378 cam.setCameraPointing(0.0
f, .0
f, .0
f);
379 cam.setCameraProjective(!m_gui_options.ortho);
380 cam.setZoomDistance(m_gui_options.camera_distance);
384 m_gui.gui_win->drawAll();
385 m_gui.gui_win->setVisible(
true);
388 m_gui.gui_win->setKeyboardCallback([&](
int key,
int ,
389 int action,
int modifiers) {
390 if (action != GLFW_PRESS && action != GLFW_REPEAT)
return false;
392 auto lck = mrpt::lockHelper(m_lastKeyEvent_mtx);
394 m_lastKeyEvent.keycode = key;
395 m_lastKeyEvent.modifierShift = (modifiers & GLFW_MOD_SHIFT) != 0;
396 m_lastKeyEvent.modifierCtrl = (modifiers & GLFW_MOD_CONTROL) != 0;
397 m_lastKeyEvent.modifierSuper = (modifiers & GLFW_MOD_SUPER) != 0;
398 m_lastKeyEvent.modifierAlt = (modifiers & GLFW_MOD_ALT) != 0;
400 m_lastKeyEventValid =
true;
405 m_gui_thread_running =
true;
409 m_gui.gui_win->setLoopCallback([&]() {
410 if (m_gui_thread_must_close) nanogui::leave();
413 ASSERT_(m_gui.gui_win->background_scene);
414 internalUpdate3DSceneObjects(m_gui.gui_win->background_scene);
416 internal_process_pending_gui_user_tasks();
421 mrpt::img::CImage im;
424 m_gui.gui_win->background_scene->getViewport()->getCamera();
425 auto camBackup = cam;
426 cam.set6DOFMode(
true);
428 this->m_vehicles.begin()->second->getPose());
429 using namespace mrpt;
434 fbo.
getFrame(*m_gui.gui_win->background_scene, im);
443 m_gui.handle_mouse_operations();
446 const int refresh_ms =
447 std::max(1,
mrpt::round(1000 / m_gui_options.refresh_fps));
450 "[World::internal_GUI_thread] Using GUI FPS=%i (T=%i ms)",
451 m_gui_options.refresh_fps, refresh_ms);
453 nanogui::mainloop(refresh_ms);
457 m_gui.gui_win.reset();
461 catch (
const std::exception& e)
464 "[internal_GUI_init] Exception: " << mrpt::exception_to_str(e));
466 m_gui_thread_running =
false;
472 if (!gui_win)
return;
474 mrpt::opengl::COpenGLViewport::Ptr vp;
476 auto lck = mrpt::lockHelper(gui_win->background_scene_mtx);
477 vp = gui_win->background_scene->getViewport();
481 const auto mousePt = gui_win->mousePos();
483 vp->get3DRayForPixelCoord(mousePt.x(), mousePt.y(), ray);
486 const auto ground_plane =
487 mrpt::math::TPlane::From3Points({0, 0, 0}, {1, 0, 0}, {0, 1, 0});
495 if (inters.getPoint(clickedPt))
501 #if MRPT_VERSION >= 0x211 502 const auto screen = gui_win->screen();
503 const bool leftClick = screen->mouseState() == 0x01;
506 if (btnReplaceObject && btnReplaceObject->pushed())
508 static bool isReplacing =
false;
511 if (!isReplacing && !leftClick)
515 if (gui_selectedObject.simulable)
523 gui_selectedObject.simulable->setPose(p);
525 if (isReplacing && leftClick)
528 btnReplaceObject->setPushed(
false);
538 m_gui_user_pending_tasks_mtx.lock();
540 for (
const auto& task : m_gui_user_pending_tasks)
544 m_gui_user_pending_tasks.clear();
546 m_gui_user_pending_tasks_mtx.unlock();
550 mrpt::opengl::COpenGLScene::Ptr& gl_scene)
554 m_timlogger.enter(
"update_GUI.2.map-elements");
556 for (
auto& e : m_world_elements) e->guiUpdate(*gl_scene);
558 m_timlogger.leave(
"update_GUI.2.map-elements");
562 m_timlogger.enter(
"update_GUI.3.vehicles");
564 for (
auto&
v : m_vehicles)
v.second->guiUpdate(*gl_scene);
566 m_timlogger.leave(
"update_GUI.3.vehicles");
570 m_timlogger.enter(
"update_GUI.4.blocks");
572 for (
auto&
v : m_blocks)
v.second->guiUpdate(*gl_scene);
574 m_timlogger.leave(
"update_GUI.4.blocks");
578 m_timlogger.enter(
"update_GUI.5.text-msgs");
581 double cpu_usage_ratio =
582 std::max(1e-10, m_timlogger.getMeanTime(
"run_simulation.cpu_dt")) /
583 std::max(1e-10, m_timlogger.getMeanTime(
"run_simulation.dt"));
585 if (m_gui.lbCpuUsage)
587 "Time: %s (CPU usage: %.03f%%)",
589 cpu_usage_ratio * 100.0));
592 m_gui_msg_lines_mtx.lock();
593 const std::string msg_lines = m_gui_msg_lines;
594 m_gui_msg_lines_mtx.unlock();
596 if (!msg_lines.empty())
599 m_gui.lbStatuses[0]->setCaption(msg_lines);
601 m_gui.lbStatuses[1]->setCaption(
602 std::string(
"Mouse: ") + m_gui.clickedPt.asString());
605 m_timlogger.leave(
"update_GUI.5.text-msgs");
609 if (!m_gui_options.follow_vehicle.empty())
611 auto it = m_vehicles.find(m_gui_options.follow_vehicle);
612 if (it == m_vehicles.end())
614 static bool warn1st =
true;
618 "GUI: Camera set to follow vehicle named '%s' which " 621 m_gui_options.follow_vehicle.c_str());
628 m_gui.gui_win->camera().setCameraPointing(pose.
x(), pose.
y(), 0.0f);
638 auto lock = mrpt::lockHelper(m_gui_thread_start_mtx);
639 if (!m_gui_thread_running && !m_gui_thread.joinable())
644 #if MRPT_VERSION >= 0x204 645 mrpt::system::thread_name(
"guiThread", m_gui_thread);
647 for (
int timeout = 0; timeout < 300; timeout++)
649 std::this_thread::sleep_for(std::chrono::milliseconds(10));
650 if (m_gui_thread_running)
break;
653 if (!m_gui_thread_running)
668 "[World::update_GUI] GUI window has been closed, but note that " 669 "simulation keeps running.");
673 m_timlogger.enter(
"update_GUI");
676 m_gui_msg_lines_mtx.lock();
678 m_gui_msg_lines_mtx.unlock();
680 m_timlogger.leave(
"update_GUI");
684 if (guiparams && m_lastKeyEventValid)
686 auto lck = mrpt::lockHelper(m_lastKeyEvent_mtx);
688 guiparams->
keyevent = std::move(m_lastKeyEvent);
689 m_lastKeyEventValid =
false;
void getFrame(const COpenGLScene &scene, mrpt::utils::CImage &image)
void prepare_editor_window()
This file contains rapidxml parser and DOM implementation.
static Ptr factory(World *parent, const rapidxml::xml_node< char > *xml_node, const char *class_name=nullptr)
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const TParameterDefinitions ¶ms, const std::map< std::string, std::string > &variableNamesValues={}, const char *functionNameContext="")
#define THROW_EXCEPTION(msg)
GLubyte GLubyte GLubyte GLubyte w
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
void internalUpdate3DSceneObjects(mrpt::opengl::COpenGLScene::Ptr &gl_scene)
std::string BASE_IMPEXP formatTimeInterval(const double timeSeconds)
#define MRPT_LOG_DEBUG(_STRING)
GLsizei const GLfloat * value
MRPT_TODO("Modify ping to run on Windows + Test this")
TGUIKeyEvent keyevent
Keystrokes in the window are returned here.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
void internal_GUI_thread()
bool is_GUI_open() const
Forces closing the GUI window, if any.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
GLuint const GLchar * name
std::string msg_lines
Messages to show.
void showBoundingBox(bool show)
void handle_mouse_operations()
void prepare_status_window()
void parse_from(const rapidxml::xml_node< char > &node)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
GLdouble GLdouble GLdouble b
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
void update_GUI(TUpdateGUIParams *params=nullptr)
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
void internal_process_pending_gui_user_tasks()
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
void close_GUI()
a previous call to update_GUI()