World_gui.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014-2023 Jose Luis Blanco Claraco |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under 3-clause BSD License |
7  | See COPYING |
8  +-------------------------------------------------------------------------+ */
9 
10 #include <mrpt/core/format.h>
11 #include <mrpt/core/get_env.h>
12 #include <mrpt/core/lock_helper.h>
13 #include <mrpt/core/round.h>
14 #include <mrpt/math/TLine3D.h>
15 #include <mrpt/math/TObject3D.h>
16 #include <mrpt/math/geometry.h>
17 #include <mrpt/obs/CObservation3DRangeScan.h>
18 #include <mrpt/obs/CObservationImage.h>
19 #include <mrpt/opengl/COpenGLScene.h>
20 #include <mrpt/version.h>
21 #include <mvsim/World.h>
22 
23 #include <cmath> // cos(), sin()
24 #include <rapidxml.hpp>
25 
26 #include "xml_utils.h"
27 #if MRPT_VERSION >= 0x204
28 #include <mrpt/system/thread_name.h>
29 #endif
30 
31 using namespace mvsim;
32 using namespace std;
33 
34 void World::TGUI_Options::parse_from(
35  const rapidxml::xml_node<char>& node, mrpt::system::COutputLogger& logger)
36 {
37  parse_xmlnode_children_as_param(
38  node, params, {}, "[World::TGUI_Options]", &logger);
39 }
40 
41 void World::LightOptions::parse_from(
42  const rapidxml::xml_node<char>& node, mrpt::system::COutputLogger& logger)
43 {
44  parse_xmlnode_children_as_param(
45  node, params, {}, "[World::LightOptions]", &logger);
46 }
47 
50 bool World::is_GUI_open() const { return !!gui_.gui_win; }
52 void World::close_GUI() { gui_.gui_win.reset(); }
53 
54 // Add top menu subwindow:
55 void World::GUI::prepare_control_window()
56 {
57 #if MRPT_VERSION >= 0x211
58  nanogui::Window* w = gui_win->createManagedSubWindow("Control");
59 #else
60  nanogui::Window* w = new nanogui::Window(gui_win.get(), "Control");
61 #endif
62 
63  // Place control UI at the top-left corner:
64  gui_win->getSubWindowsUI()->setPosition({1, 1});
65 
66  w->setPosition({1, 80});
67  w->setLayout(new nanogui::BoxLayout(
68  nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 5));
69 
70  w->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
71  ->setCallback([this]() {
72  parent_.simulator_must_close(true);
73  gui_win->setVisible(false);
74  nanogui::leave();
75  });
76 
77  std::vector<std::string> lstVehicles;
78  lstVehicles.reserve(parent_.vehicles_.size() + 1);
79 
80  lstVehicles.push_back("[none]"); // None
81  for (const auto& v : parent_.vehicles_) lstVehicles.push_back(v.first);
82 
83  w->add<nanogui::Label>("Camera follows:");
84  auto cbFollowVeh = w->add<nanogui::ComboBox>(lstVehicles);
85  cbFollowVeh->setSelectedIndex(0);
86  cbFollowVeh->setCallback([this, lstVehicles](int idx) {
87  if (idx == 0)
88  parent_.guiOptions_.follow_vehicle.clear();
89  else if (idx <= static_cast<int>(parent_.vehicles_.size()))
90  parent_.guiOptions_.follow_vehicle = lstVehicles[idx];
91  });
92 
93  w->add<nanogui::CheckBox>("Orthogonal view", [&](bool b) {
94  gui_win->camera().setCameraProjective(!b);
95  })->setChecked(parent_.guiOptions_.ortho);
96 
97 #if MRPT_VERSION >= 0x270
98  w->add<nanogui::CheckBox>("Enable shadows", [&](bool b) {
99  auto vv = parent_.worldVisual_->getViewport();
100  auto vp = parent_.worldPhysical_.getViewport();
101  vv->enableShadowCasting(b);
102  vp->enableShadowCasting(b);
103  parent_.lightOptions_.enable_shadows = b;
104  })->setChecked(parent_.lightOptions_.enable_shadows);
105 #endif
106 
107  w->add<nanogui::Label>("Light azimuth:");
108  {
109  auto sl = w->add<nanogui::Slider>();
110  sl->setRange({-M_PI, M_PI});
111  sl->setValue(parent_.lightOptions_.light_azimuth);
112  sl->setCallback([this](float v) {
113  parent_.lightOptions_.light_azimuth = v;
114  parent_.setLightDirectionFromAzimuthElevation(
115  parent_.lightOptions_.light_azimuth,
116  parent_.lightOptions_.light_elevation);
117  });
118  }
119  w->add<nanogui::Label>("Light elevation:");
120  {
121  auto sl = w->add<nanogui::Slider>();
122  sl->setRange({0, M_PI * 0.5});
123  sl->setValue(parent_.lightOptions_.light_elevation);
124  sl->setCallback([this](float v) {
125  parent_.lightOptions_.light_elevation = v;
126  parent_.setLightDirectionFromAzimuthElevation(
127  parent_.lightOptions_.light_azimuth,
128  parent_.lightOptions_.light_elevation);
129  });
130  }
131 
132  w->add<nanogui::CheckBox>("View forces", [&](bool b) {
133  parent_.guiOptions_.show_forces = b;
134  })->setChecked(parent_.guiOptions_.show_forces);
135 
136  w->add<nanogui::CheckBox>("View sensor pointclouds", [&](bool b) {
137  std::lock_guard<std::mutex> lck(gui_win->background_scene_mtx);
138 
139  auto glVizSensors =
140  std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
141  gui_win->background_scene->getByName("group_sensors_viz"));
142  ASSERT_(glVizSensors);
143 
144  glVizSensors->setVisibility(b);
145  })->setChecked(parent_.guiOptions_.show_sensor_points);
146 
147  w->add<nanogui::CheckBox>("View sensor poses", [&](bool b) {
148  const auto& objs = SensorBase::GetAllSensorsOriginViz();
149  for (const auto& o : *objs) o->setVisibility(b);
150  })->setChecked(false);
151 
152  w->add<nanogui::CheckBox>("View sensor FOVs", [&](bool b) {
153  const auto& objs = SensorBase::GetAllSensorsFOVViz();
154  for (const auto& o : *objs) o->setVisibility(b);
155  })->setChecked(false);
156 
157  w->add<nanogui::CheckBox>("View collision shapes", [&](bool b) {
158  auto lck = mrpt::lockHelper(parent_.simulableObjectsMtx_);
159  for (auto& s : parent_.simulableObjects_)
160  {
161  auto* vis = dynamic_cast<VisualObject*>(s.second.get());
162  if (!vis) continue;
163  vis->showCollisionShape(b);
164  }
165  })->setChecked(false);
166 }
167 
168 // Add Status window
169 void World::GUI::prepare_status_window()
170 {
171 #if MRPT_VERSION >= 0x211
172  nanogui::Window* w = gui_win->createManagedSubWindow("Status");
173 #else
174  nanogui::Window* w = new nanogui::Window(gui_win.get(), "Status");
175 #endif
176 
177  w->setPosition({5, 455});
178  w->setLayout(new nanogui::BoxLayout(
179  nanogui::Orientation::Vertical, nanogui::Alignment::Fill));
180  w->setFixedWidth(320);
181 
182 #if MRPT_VERSION < 0x211
183  w->buttonPanel()
184  ->add<nanogui::Button>("", ENTYPO_ICON_CROSS)
185  ->setCallback([w]() { w->setVisible(false); });
186 #endif
187 
188  lbCpuUsage = w->add<nanogui::Label>(" ");
189  lbStatuses.resize(12);
190  for (size_t i = 0; i < lbStatuses.size(); i++)
191  lbStatuses[i] = w->add<nanogui::Label>(" ");
192 }
193 
194 // Add editor window
195 void World::GUI::prepare_editor_window()
196 {
197 #if MRPT_VERSION >= 0x211
198 #if MRPT_VERSION >= 0x231
199  const auto subwinIdx = gui_win->getSubwindowCount();
200 #endif
201  nanogui::Window* w = gui_win->createManagedSubWindow("Editor");
202 #else
203  nanogui::Window* w = new nanogui::Window(gui_win.get(), "Editor");
204 #endif
205 
206  const int pnWidth = 250, pnHeight = 200;
207 
208  w->setPosition({1, 230});
209  w->setLayout(new nanogui::BoxLayout(
210  nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 3, 3));
211  w->setFixedWidth(pnWidth);
212 
213 #if MRPT_VERSION < 0x211
214  w->buttonPanel()
215  ->add<nanogui::Button>("", ENTYPO_ICON_CROSS)
216  ->setCallback([w]() { w->setVisible(false); });
217 #endif
218 
219  w->add<nanogui::Label>("Selected object", "sans-bold");
220 
221  auto lckListObjs = mrpt::lockHelper(parent_.getListOfSimulableObjectsMtx());
222  if (!parent_.getListOfSimulableObjects().empty())
223  {
224  auto tab = w->add<nanogui::TabWidget>();
225 
226  nanogui::Widget* tabs[4] = {
227  tab->createTab("Vehicles"), tab->createTab("Blocks"),
228  tab->createTab("Elements"), tab->createTab("Misc.")};
229 
230  tab->setActiveTab(0);
231 
232  for (auto t : tabs)
233  t->setLayout(new nanogui::BoxLayout(
234  nanogui::Orientation::Vertical, nanogui::Alignment::Minimum, 3,
235  3));
236 
237  nanogui::VScrollPanel* vscrolls[4] = {
238  tabs[0]->add<nanogui::VScrollPanel>(),
239  tabs[1]->add<nanogui::VScrollPanel>(),
240  tabs[2]->add<nanogui::VScrollPanel>(),
241  tabs[3]->add<nanogui::VScrollPanel>()};
242 
243  for (auto vs : vscrolls) vs->setFixedSize({pnWidth, pnHeight});
244 
245  // vscroll should only have *ONE* child. this is what `wrapper`
246  // is for
247  nanogui::Widget* wrappers[4];
248  for (int i = 0; i < 4; i++)
249  {
250  wrappers[i] = vscrolls[i]->add<nanogui::Widget>();
251  wrappers[i]->setFixedSize({pnWidth, pnHeight});
252  wrappers[i]->setLayout(new nanogui::GridLayout(
253  nanogui::Orientation::Horizontal, 1 /*columns */,
254  nanogui::Alignment::Minimum, 3, 3));
255  }
256 
257  for (const auto& o : parent_.getListOfSimulableObjects())
258  {
259  InfoPerObject ipo;
260 
261  const auto& name = o.first;
262  bool isVehicle = false;
263  if (auto v = dynamic_cast<VehicleBase*>(o.second.get()); v)
264  {
265  isVehicle = true;
266  ipo.visual = dynamic_cast<VisualObject*>(v);
267  }
268  bool isBlock = false;
269  if (auto v = dynamic_cast<Block*>(o.second.get()); v)
270  {
271  isBlock = true;
272  ipo.visual = dynamic_cast<VisualObject*>(v);
273  }
274  // bool isWorldElement = false;
275  if (auto v = dynamic_cast<WorldElementBase*>(o.second.get()); v)
276  {
277  // isWorldElement = true;
278  ipo.visual = dynamic_cast<VisualObject*>(v);
279  }
280  auto wrapper =
281  isVehicle ? wrappers[0] : (isBlock ? wrappers[1] : wrappers[2]);
282 
283  std::string label = name;
284  if (label.empty()) label = "(unnamed)";
285 
286  auto cb = wrapper->add<nanogui::CheckBox>(label);
287  ipo.cb = cb;
288  ipo.simulable = o.second;
289  gui_cbObjects.emplace_back(ipo);
290 
291  cb->setChecked(false);
292  cb->setCallback([cb, ipo, this](bool check) {
293  // deselect former one:
294  if (gui_selectedObject.visual)
295  gui_selectedObject.visual->showCollisionShape(false);
296  if (gui_selectedObject.cb)
297  gui_selectedObject.cb->setChecked(false);
298  gui_selectedObject = InfoPerObject();
299 
300  cb->setChecked(check);
301 
302  // If checked, show bounding box:
303  if (ipo.visual && check)
304  {
305  gui_selectedObject = ipo;
306  ipo.visual->showCollisionShape(true);
307  }
308 
309  const bool btnsEnabled = !!gui_selectedObject.simulable;
310  for (auto b : btns_selectedOps) b->setEnabled(btnsEnabled);
311  });
312  }
313 
314  // "misc." tab
315  // --------------
316  wrappers[3]
317  ->add<nanogui::Button>("Save 3D scene...", ENTYPO_ICON_EXPORT)
318  ->setCallback([this]() {
319  try
320  {
321  const std::string outFile = nanogui::file_dialog(
322  {{"3Dscene", "MRPT 3D scene file (*.3Dsceme)"}},
323  true /*save*/);
324  if (outFile.empty()) return;
325 
326  auto lck = mrpt::lockHelper(parent_.physical_objects_mtx());
327  parent_.worldPhysical_.saveToFile(outFile);
328 
329  std::cout << "[mvsim gui] Saved world scene to: " << outFile
330  << std::endl;
331  }
332  catch (const std::exception& e)
333  {
334  std::cerr
335  << "[mvsim gui] Exception while saving 3D scene:\n"
336  << e.what() << std::endl;
337  }
338  });
339  }
340 
341  w->add<nanogui::Label>(" ");
342 
343  btnReplaceObject = w->add<nanogui::Button>("Click to replace...");
344  btnReplaceObject->setFlags(nanogui::Button::Flags::ToggleButton);
345  btns_selectedOps.push_back(btnReplaceObject);
346 
347  {
348  auto pn = w->add<nanogui::Widget>();
349  pn->setLayout(new nanogui::BoxLayout(
350  nanogui::Orientation::Horizontal, nanogui::Alignment::Fill, 2, 2));
351  pn->add<nanogui::Label>("Reorient:");
352  auto slAngle = pn->add<nanogui::Slider>();
353  slAngle->setRange({-M_PI, M_PI});
354  slAngle->setCallback([this](float v) {
355  if (!gui_selectedObject.simulable) return;
356  auto p = gui_selectedObject.simulable->getPose();
357  p.yaw = v;
358  gui_selectedObject.simulable->setPose(p);
359  });
360  slAngle->setFixedWidth(150);
361  btns_selectedOps.push_back(slAngle);
362  }
363 
364  auto btnPlaceCoords = w->add<nanogui::Button>("Replace by coordinates...");
365  btns_selectedOps.push_back(btnPlaceCoords);
366  btnPlaceCoords->setCallback([this]() {
367  //
368  if (!gui_selectedObject.simulable) return;
369 
370  auto* formPose = new nanogui::Window(gui_win.get(), "Enter new pose");
371  formPose->setLayout(new nanogui::GridLayout(
372  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5));
373 
374  nanogui::TextBox* lbs[3];
375 
376  formPose->add<nanogui::Label>("x coordinate:");
377  lbs[0] = formPose->add<nanogui::TextBox>();
378  formPose->add<nanogui::Label>("y coordinate:");
379  lbs[1] = formPose->add<nanogui::TextBox>();
380  formPose->add<nanogui::Label>("Orientation:");
381  lbs[2] = formPose->add<nanogui::TextBox>();
382 
383  for (int i = 0; i < 3; i++)
384  {
385  lbs[i]->setEditable(true);
386  lbs[i]->setFixedSize({100, 20});
387  lbs[i]->setValue("0.0");
388  lbs[i]->setUnits(i == 2 ? "[deg]" : "[m]");
389  lbs[i]->setDefaultValue("0.0");
390  lbs[i]->setFontSize(16);
391  lbs[i]->setFormat("[-]?[0-9]*\\.?[0-9]+");
392  }
393 
394  const auto pos = gui_selectedObject.simulable->getPose();
395  lbs[0]->setValue(std::to_string(pos.x));
396  lbs[1]->setValue(std::to_string(pos.y));
397  lbs[2]->setValue(std::to_string(mrpt::RAD2DEG(pos.yaw)));
398 
399  formPose->add<nanogui::Label>("");
400  formPose->add<nanogui::Label>("");
401 
402  formPose->add<nanogui::Button>("Cancel")->setCallback(
403  [formPose]() { formPose->dispose(); });
404 
405  formPose->add<nanogui::Button>("Accept")->setCallback(
406  [formPose, this, lbs]() {
407  gui_selectedObject.simulable->setPose(
408  {// X:
409  std::stod(lbs[0]->value()),
410  // Y:
411  std::stod(lbs[1]->value()),
412  // Z:
413  .0,
414  // Yaw
415  mrpt::DEG2RAD(std::stod(lbs[2]->value())),
416  // Pitch
417  0.0,
418  // Roll:
419  0.0});
420  formPose->dispose();
421  });
422 
423  formPose->setModal(true);
424  formPose->center();
425  formPose->setVisible(true);
426  });
427 
428  for (auto b : btns_selectedOps) b->setEnabled(false);
429 
430  // Minimize subwindow:
431 #if MRPT_VERSION >= 0x231
432  gui_win->subwindowMinimize(subwinIdx);
433 #else
434  if (auto btnMinimize =
435  dynamic_cast<nanogui::Button*>(w->buttonPanel()->children().at(0));
436  btnMinimize)
437  {
438  btnMinimize->callback()(); // "push" button
439  }
440 #endif
441 
442 } // end "editor" window
443 
444 void World::internal_GUI_thread()
445 {
446  try
447  {
448  MRPT_LOG_DEBUG("[World::internal_GUI_thread] Started.");
449 
450  // Start GUI:
451  nanogui::init();
452 
453  mrpt::gui::CDisplayWindowGUI_Params cp;
454  cp.maximized = guiOptions_.start_maximized;
455 
456  gui_.gui_win = mrpt::gui::CDisplayWindowGUI::Create(
457  "mvsim", guiOptions_.win_w, guiOptions_.win_h, cp);
458 
459  // zmin / zmax of opengl viewport:
460  worldVisual_->getViewport()->setViewportClipDistances(
461  guiOptions_.clip_plane_min, guiOptions_.clip_plane_max);
462 
463  // Add a background scene:
464  {
465  // we use the member scene worldVisual_ as the placeholder for the
466  // visual 3D scene:
467 
468  // add the placeholders for user-provided objects, both for pure
469  // visualization only, and physical objects:
470  worldVisual_->insert(glUserObjsViz_);
471  worldPhysical_.insert(glUserObjsPhysical_);
472 
473  std::lock_guard<std::mutex> lck(gui_.gui_win->background_scene_mtx);
474  gui_.gui_win->background_scene = worldVisual_;
475  }
476 
477  // Only if the world is empty: at least introduce a ground grid:
478  if (worldElements_.empty())
479  {
480  auto we = WorldElementBase::factory(this, nullptr, "ground_grid");
481  worldElements_.push_back(we);
482  }
483 
484  // Windows:
485  gui_.prepare_control_window();
486  gui_.prepare_status_window();
487  gui_.prepare_editor_window();
488 
489  // Finish GUI setup:
490  gui_.gui_win->performLayout();
491  auto& cam = gui_.gui_win->camera();
492 
493  cam.setCameraPointing(0.0f, .0f, .0f);
494  cam.setCameraProjective(!guiOptions_.ortho);
495  cam.setZoomDistance(guiOptions_.camera_distance);
496  cam.setAzimuthDegrees(guiOptions_.camera_azimuth_deg);
497  cam.setElevationDegrees(guiOptions_.camera_elevation_deg);
498  cam.setCameraFOV(guiOptions_.fov_deg);
499  cam.setCameraPointing(
500  guiOptions_.camera_point_to.x, guiOptions_.camera_point_to.y,
501  guiOptions_.camera_point_to.z);
502 
503  const auto& lo = lightOptions_;
504 
505  setLightDirectionFromAzimuthElevation(
506  lo.light_azimuth, lo.light_elevation);
507 
508 #if MRPT_VERSION >= 0x270
509  auto vv = worldVisual_->getViewport();
510  auto vp = worldPhysical_.getViewport();
511 
512  auto lambdaSetLightParams =
513  [&lo](const mrpt::opengl::COpenGLViewport::Ptr& v) {
514  // enable shadows and set the shadow map texture size:
515  const int sms = lo.shadow_map_size;
516  v->enableShadowCasting(lo.enable_shadows, sms, sms);
517 
518  // light color:
519  const auto colf = mrpt::img::TColorf(lo.light_color);
520 
521  auto& vlp = v->lightParameters();
522 
523  vlp.color = colf;
524 
525 #if MRPT_VERSION >= 0x2A0 // New in mrpt>=2.10.0
526  vlp.eyeDistance2lightShadowExtension =
527  lo.eye_distance_to_shadow_map_extension;
528 
529  vlp.minimum_shadow_map_extension_ratio =
530  lo.minimum_shadow_map_extension_ratio;
531 #endif
532  // light view frustrum near/far planes:
533  v->setLightShadowClipDistances(
534  lo.light_clip_plane_min, lo.light_clip_plane_max);
535 
536  // Shadow bias should be proportional to clip range:
537 #if MRPT_VERSION >= 0x281
538  vlp.shadow_bias = lo.shadow_bias;
539  vlp.shadow_bias_cam2frag = lo.shadow_bias_cam2frag;
540  vlp.shadow_bias_normal = lo.shadow_bias_normal;
541 #endif
542  };
543 
544  lambdaSetLightParams(vv);
545  lambdaSetLightParams(vp);
546 #endif
547 
548  // Main GUI loop
549  // ---------------------
550  gui_.gui_win->drawAll();
551  gui_.gui_win->setVisible(true);
552 
553  // Listen for keyboard events:
554 #if MRPT_VERSION >= 0x232
555  gui_.gui_win->addKeyboardCallback(
556 #else
557  gui_.gui_win->setKeyboardCallback(
558 #endif
559  [&](int key, int /*scancode*/, int action, int modifiers) {
560  if (action != GLFW_PRESS && action != GLFW_REPEAT) return false;
561 
562  auto lck = mrpt::lockHelper(lastKeyEventMtx_);
563 
564  lastKeyEvent_.keycode = key;
565  lastKeyEvent_.modifierShift = (modifiers & GLFW_MOD_SHIFT) != 0;
566  lastKeyEvent_.modifierCtrl =
567  (modifiers & GLFW_MOD_CONTROL) != 0;
568  lastKeyEvent_.modifierSuper = (modifiers & GLFW_MOD_SUPER) != 0;
569  lastKeyEvent_.modifierAlt = (modifiers & GLFW_MOD_ALT) != 0;
570 
571  lastKeyEventValid_ = true;
572 
573  return false;
574  });
575 
576  gui_thread_running_ = true;
577 
578  // The GUI must be closed from this same thread. Use a shared atomic
579  // bool:
580  auto lambdaLoopCallback = [](World& me) {
581  if (me.simulator_must_close()) nanogui::leave();
582 
583  try
584  {
585  // Update 3D vehicles, sensors, run render-based sensors, etc:
586  me.internalGraphicsLoopTasksForSimulation();
587 
588  me.internal_process_pending_gui_user_tasks();
589 
590  // handle mouse operations:
591  me.gui_.handle_mouse_operations();
592  }
593  catch (const std::exception& e)
594  {
595  // In case of an exception in the functions above,
596  // abort. Otherwise, the error may repeat over and over forever
597  // and the main thread will never know about it.
598  me.logStr(mrpt::system::LVL_ERROR, e.what());
599  me.simulator_must_close(true);
600  me.gui_.gui_win->setVisible(false);
601  nanogui::leave();
602  }
603  };
604 
605 #if MRPT_VERSION >= 0x232
606  gui_.gui_win->addLoopCallback(
607 #else
608  gui_.gui_win->setLoopCallback(
609 #endif
610  [=]() { lambdaLoopCallback(*this); });
611 
612  // Register observation callback:
613  const auto lambdaOnObservation =
614  [this](
615  const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) {
616  // obs->getDescriptionAsText(std::cout);
617  this->enqueue_task_to_run_in_gui_thread([this, obs, &veh]() {
618  internal_gui_on_observation(veh, obs);
619  });
620  };
621 
622  this->registerCallbackOnObservation(lambdaOnObservation);
623 
624  // ============= Mainloop =============
625  const int refresh_ms =
626  std::max(1, mrpt::round(1000 / guiOptions_.refresh_fps));
627 
628  MRPT_LOG_DEBUG_FMT(
629  "[World::internal_GUI_thread] Using GUI FPS=%i (T=%i ms)",
630  guiOptions_.refresh_fps, refresh_ms);
631 
632 #if MRPT_VERSION >= 0x253
633  const int idleLoopTasks_ms = 10;
634 
635  nanogui::mainloop(idleLoopTasks_ms, refresh_ms);
636 #else
637  nanogui::mainloop(refresh_ms);
638 #endif
639 
640  MRPT_LOG_DEBUG("[World::internal_GUI_thread] Mainloop ended.");
641 
642  // to let other threads know that we are closing:
643  simulator_must_close(true);
644 
645  // Make sure opengl resources are freed from this thread, not from
646  // the main one upon destruction of the last ref to shared_ptr's to
647  // opengl classes.
648  {
649  auto lck = mrpt::lockHelper(gui_.gui_win->background_scene_mtx);
650  if (gui_.gui_win->background_scene)
651  gui_.gui_win->background_scene->freeOpenGLResources();
652  }
653 
654  auto lckListObjs = mrpt::lockHelper(getListOfSimulableObjectsMtx());
655 
656  for (auto& obj : getListOfSimulableObjects())
657  obj.second->freeOpenGLResources();
658 
659  lckListObjs.unlock();
660 
661  VisualObject::FreeOpenGLResources();
662 
663  // Now, destroy window:
664  gui_.gui_win.reset();
665 
666  nanogui::shutdown();
667  }
668  catch (const std::exception& e)
669  {
670  MRPT_LOG_ERROR_STREAM(
671  "[internal_GUI_init] Exception: " << mrpt::exception_to_str(e));
672  }
673  gui_thread_running_ = false;
674 }
675 
676 void World::GUI::handle_mouse_operations()
677 {
678  MRPT_START
679  if (!gui_win) return;
680 
681  mrpt::opengl::COpenGLViewport::Ptr vp;
682  {
683  auto lck = mrpt::lockHelper(gui_win->background_scene_mtx);
684  if (!gui_win->background_scene) return;
685  vp = gui_win->background_scene->getViewport();
686  }
687  ASSERT_(vp);
688 
689  const auto mousePt = gui_win->mousePos();
690  mrpt::math::TLine3D ray;
691  vp->get3DRayForPixelCoord(mousePt.x(), mousePt.y(), ray);
692 
693  // Create a 3D plane, i.e. Z=0
694  const auto ground_plane =
695  mrpt::math::TPlane::From3Points({0, 0, 0}, {1, 0, 0}, {0, 1, 0});
696 
697  // Intersection of the line with the plane:
698  mrpt::math::TObject3D inters;
699  mrpt::math::intersect(ray, ground_plane, inters);
700 
701  // Interpret the intersection as a point, if there is an
702  // intersection:
703  if (inters.getPoint(clickedPt))
704  {
705  // Move object to the position picked by the user:
706  // vp->getByClass<CDisk>(0)->setLocation(clickedPt);
707  }
708 
709 #if MRPT_VERSION >= 0x211
710  const auto screen = gui_win->screen();
711  const bool leftClick = screen->mouseState() == 0x01;
712 
713  // Replace object?
714  if (btnReplaceObject && btnReplaceObject->pushed())
715  {
716  static bool isReplacing = false;
717 
718  // Start of replace? When the button push is released:
719  if (!isReplacing && !leftClick)
720  {
721  isReplacing = true;
722  }
723  if (gui_selectedObject.simulable)
724  {
725  // btnReplaceObject->screen()->setCursor()
726 
727  mrpt::math::TPose3D p = gui_selectedObject.simulable->getPose();
728  p.x = clickedPt.x;
729  p.y = clickedPt.y;
730 
731  gui_selectedObject.simulable->setPose(p);
732  }
733  if (isReplacing && leftClick)
734  {
735  isReplacing = false;
736  btnReplaceObject->setPushed(false);
737  }
738  }
739 #endif
740 
741  MRPT_END
742 }
743 
744 void World::internal_process_pending_gui_user_tasks()
745 {
746  guiUserPendingTasksMtx_.lock();
747 
748  for (const auto& task : guiUserPendingTasks_)
749  {
750  task();
751  }
752  guiUserPendingTasks_.clear();
753 
754  guiUserPendingTasksMtx_.unlock();
755 }
756 
757 void World::internalRunSensorsOn3DScene(
758  mrpt::opengl::COpenGLScene& physicalObjects)
759 {
760  auto tle = mrpt::system::CTimeLoggerEntry(
761  timlogger_, "internalRunSensorsOn3DScene");
762 
763  for (auto& v : vehicles_)
764  for (auto& sensor : v.second->getSensors())
765  if (sensor) sensor->simulateOn3DScene(physicalObjects);
766 
767  // clear the flag of pending 3D simulation required:
768  clear_pending_running_sensors_on_3D_scene();
769 }
770 
771 void World::internalUpdate3DSceneObjects(
772  mrpt::opengl::COpenGLScene& viz, mrpt::opengl::COpenGLScene& physical)
773 {
774  // Update view of map elements
775  // -----------------------------
776  auto tle =
777  mrpt::system::CTimeLoggerEntry(timlogger_, "update_GUI.2.map-elements");
778 
779  for (auto& e : worldElements_) e->guiUpdate(viz, physical);
780 
781  tle.stop();
782 
783  // Update view of vehicles
784  // -----------------------------
785  timlogger_.enter("update_GUI.3.vehicles");
786 
787  for (auto& v : vehicles_) v.second->guiUpdate(viz, physical);
788 
789  timlogger_.leave("update_GUI.3.vehicles");
790 
791  // Update view of blocks
792  // -----------------------------
793  timlogger_.enter("update_GUI.4.blocks");
794 
795  for (auto& v : blocks_) v.second->guiUpdate(viz, physical);
796 
797  timlogger_.leave("update_GUI.4.blocks");
798 
799  // Other messages
800  // -----------------------------
801  timlogger_.enter("update_GUI.5.text-msgs");
802  if (gui_.lbCpuUsage)
803  {
804  // 1st line: time
805  double cpu_usage_ratio =
806  std::max(1e-10, timlogger_.getMeanTime("run_simulation.cpu_dt")) /
807  std::max(1e-10, timlogger_.getMeanTime("run_simulation.dt"));
808 
809  gui_.lbCpuUsage->setCaption(mrpt::format(
810  "Time: %s (CPU usage: %.03f%%)",
811  mrpt::system::formatTimeInterval(get_simul_time()).c_str(),
812  cpu_usage_ratio * 100.0));
813 
814  // User supplied-lines:
815  guiMsgLinesMtx_.lock();
816  const std::string msg_lines = guiMsgLines_;
817  guiMsgLinesMtx_.unlock();
818 
819  int nextStatusLine = 0;
820  if (!msg_lines.empty())
821  {
822  // split lines:
823  std::vector<std::string> lines;
824  mrpt::system::tokenize(msg_lines, "\r\n", lines);
825  for (const auto& l : lines)
826  gui_.lbStatuses.at(nextStatusLine++)->setCaption(l);
827  }
828  gui_.lbStatuses.at(nextStatusLine++)
829  ->setCaption(std::string("Mouse: ") + gui_.clickedPt.asString());
830  }
831 
832  timlogger_.leave("update_GUI.5.text-msgs");
833 
834  // Camera follow modes:
835  // -----------------------
836  if (!guiOptions_.follow_vehicle.empty())
837  {
838  if (auto it = vehicles_.find(guiOptions_.follow_vehicle);
839  it != vehicles_.end())
840  {
841  const mrpt::poses::CPose2D pose = it->second->getCPose2D();
842  gui_.gui_win->camera().setCameraPointing(pose.x(), pose.y(), 0.0f);
843  }
844  else
845  {
846  MRPT_LOG_THROTTLE_ERROR_FMT(
847  5.0,
848  "GUI: Camera set to follow vehicle named '%s' which can't be "
849  "found!",
850  guiOptions_.follow_vehicle.c_str());
851  }
852  }
853 }
854 
855 void World::update_GUI(TUpdateGUIParams* guiparams)
856 {
857  // First call?
858  // -----------------------
859  {
860  auto lock = mrpt::lockHelper(gui_thread_start_mtx_);
861  if (!gui_thread_running_ && !gui_thread_.joinable())
862  {
863  MRPT_LOG_DEBUG("[update_GUI] Launching GUI thread...");
864 
865  gui_thread_ = std::thread(&World::internal_GUI_thread, this);
866 #if MRPT_VERSION >= 0x204
867  mrpt::system::thread_name("guiThread", gui_thread_);
868 #endif
869 
870  const int MVSIM_OPEN_GUI_TIMEOUT_MS =
871  mrpt::get_env<int>("MVSIM_OPEN_GUI_TIMEOUT_MS", 3000);
872 
873  for (int timeout = 0; timeout < MVSIM_OPEN_GUI_TIMEOUT_MS / 10;
874  timeout++)
875  {
876  std::this_thread::sleep_for(std::chrono::milliseconds(10));
877  if (gui_thread_running_) break;
878  }
879 
880  if (!gui_thread_running_)
881  {
882  THROW_EXCEPTION("Timeout waiting for GUI to open!");
883  }
884  else
885  {
886  MRPT_LOG_DEBUG("[update_GUI] GUI thread started.");
887  }
888  }
889  }
890 
891  if (!gui_.gui_win)
892  {
893  MRPT_LOG_THROTTLE_WARN(
894  5.0,
895  "[World::update_GUI] GUI window has been closed, but note that "
896  "simulation keeps running.");
897  return;
898  }
899 
900  timlogger_.enter("update_GUI"); // Don't count initialization, since that
901  // is a total outlier and lacks interest!
902 
903  guiMsgLinesMtx_.lock();
904  guiMsgLines_ = guiparams->msg_lines;
905  guiMsgLinesMtx_.unlock();
906 
907  timlogger_.leave("update_GUI");
908 
909  // Key-strokes:
910  // -----------------------
911  if (guiparams && lastKeyEventValid_)
912  {
913  auto lck = mrpt::lockHelper(lastKeyEventMtx_);
914 
915  guiparams->keyevent = std::move(lastKeyEvent_);
916  lastKeyEventValid_ = false;
917  }
918 }
919 
920 // This method is ensured to be run in the GUI thread
921 void World::internal_gui_on_observation(
922  const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs)
923 {
924  if (!obs) return;
925 
926  if (auto obs3D =
927  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(obs);
928  obs3D)
929  {
930  internal_gui_on_observation_3Dscan(veh, obs3D);
931  }
932  else if (auto obsIm =
933  std::dynamic_pointer_cast<mrpt::obs::CObservationImage>(obs);
934  obsIm)
935  {
936  internal_gui_on_observation_image(veh, obsIm);
937  }
938 }
939 
940 void World::internal_gui_on_observation_3Dscan(
941  const Simulable& veh,
942  const std::shared_ptr<mrpt::obs::CObservation3DRangeScan>& obs)
943 {
944  using namespace std::string_literals;
945 
946  if (!gui_.gui_win || !obs) return;
947 
948  mrpt::math::TPoint2D rgbImageWinSize = {0, 0};
949 
950  if (obs->hasIntensityImage)
951  {
952  rgbImageWinSize = internal_gui_on_image(
953  veh.getName() + "/"s + obs->sensorLabel + "_rgb"s,
954  obs->intensityImage, 5);
955  }
956  if (obs->hasRangeImage)
957  {
958  mrpt::math::CMatrixFloat d;
959  d = obs->rangeImage.asEigen().cast<float>() *
960  (obs->rangeUnits / obs->maxRange);
961 
962  mrpt::img::CImage imDepth;
963  imDepth.setFromMatrix(d, true /* in range [0,1] */);
964 
965  internal_gui_on_image(
966  veh.getName() + "/"s + obs->sensorLabel + "_depth"s, imDepth,
967  5 + 5 + rgbImageWinSize.x);
968  }
969 }
970 
971 void World::internal_gui_on_observation_image(
972  const Simulable& veh,
973  const std::shared_ptr<mrpt::obs::CObservationImage>& obs)
974 {
975  using namespace std::string_literals;
976 
977  if (!gui_.gui_win || !obs || obs->image.isEmpty()) return;
978 
979  mrpt::math::TPoint2D rgbImageWinSize = {0, 0};
980 
981  rgbImageWinSize = internal_gui_on_image(
982  veh.getName() + "/"s + obs->sensorLabel + "_rgb"s, obs->image, 5);
983 }
984 
985 mrpt::math::TPoint2D World::internal_gui_on_image(
986  const std::string& label, const mrpt::img::CImage& im, int winPosX)
987 {
988  mrpt::gui::MRPT2NanoguiGLCanvas* glControl;
989 
990  // Once creation:
991  if (!guiObsViz_.count(label))
992  {
993  auto& w = guiObsViz_[label] =
994  gui_.gui_win->createManagedSubWindow(label);
995 
996  w->setLayout(new nanogui::GridLayout(
997  nanogui::Orientation::Vertical, 1, nanogui::Alignment::Fill, 2, 2));
998 
999  // Guess window size:
1000  int winW = im.getWidth(), winH = im.getHeight();
1001 
1002  // Guess if we need to decimate subwindow size:
1003  while (winW >= 512 || winH >= 512)
1004  {
1005  winW /= 2;
1006  winH /= 2;
1007  }
1008 
1009  glControl = w->add<mrpt::gui::MRPT2NanoguiGLCanvas>();
1010  glControl->setSize({winW, winH});
1011  glControl->setFixedSize({winW, winH});
1012 
1013  static std::map<int, int> numGuiWindows;
1014  w->setPosition(
1015  {winPosX, 20 + (numGuiWindows[winPosX]++) * (winH + 10)});
1016 
1017  auto lck = mrpt::lockHelper(glControl->scene_mtx);
1018 
1019  glControl->scene = mrpt::opengl::COpenGLScene::Create();
1020  gui_.gui_win->performLayout();
1021  }
1022 
1023  // Update from sensor data:
1024  auto& w = guiObsViz_[label];
1025 
1026  glControl =
1027  dynamic_cast<mrpt::gui::MRPT2NanoguiGLCanvas*>(w->children().at(1));
1028  ASSERT_(glControl != nullptr);
1029 
1030  auto lck = mrpt::lockHelper(glControl->scene_mtx);
1031  glControl->scene->getViewport()->setImageView(im);
1032 
1033  return mrpt::math::TPoint2D(w->size().x(), w->size().y());
1034 }
1035 
1036 void World::internalGraphicsLoopTasksForSimulation()
1037 {
1038  try
1039  {
1040  // Update all GUI elements:
1041  ASSERT_(worldVisual_);
1042 
1043  auto lckPhys = mrpt::lockHelper(physical_objects_mtx());
1044 
1045  internalUpdate3DSceneObjects(*worldVisual_, worldPhysical_);
1046 
1047  internalRunSensorsOn3DScene(worldPhysical_);
1048 
1049  lckPhys.unlock();
1050 
1051  // handle user custom 3D visual objects:
1052  {
1053  const auto lck = mrpt::lockHelper(guiUserObjectsMtx_);
1054  // replace list of smart pointers (fast):
1055  if (guiUserObjectsPhysical_)
1056  *glUserObjsPhysical_ = *guiUserObjectsPhysical_;
1057  if (guiUserObjectsViz_) *glUserObjsViz_ = *guiUserObjectsViz_;
1058  }
1059  }
1060  catch (const std::exception& e)
1061  {
1062  // In case of an exception in the functions above,
1063  // abort. Otherwise, the error may repeat over and over forever
1064  // and the main thread will never know about it.
1065  MRPT_LOG_ERROR(e.what());
1066  simulator_must_close(true);
1067  }
1068 }
1069 
1070 void World::setLightDirectionFromAzimuthElevation(
1071  const float azimuth, const float elevation)
1072 {
1073  const mrpt::math::TPoint3Df dir = {
1074  -cos(azimuth) * cos(elevation), -sin(azimuth) * cos(elevation),
1075  -sin(elevation)};
1076 
1077  ASSERT_(worldVisual_);
1078 
1079  auto lckPhys = mrpt::lockHelper(physical_objects_mtx());
1080 
1081  auto vv = worldVisual_->getViewport();
1082  auto vp = worldPhysical_.getViewport();
1083 
1084  vv->lightParameters().direction = dir;
1085  vp->lightParameters().direction = dir;
1086 }
d
d
mvsim::World::GUI::prepare_editor_window
void prepare_editor_window()
Definition: World_gui.cpp:195
rapidxml.hpp
This file contains rapidxml parser and DOM implementation.
mvsim::World::LightOptions::parse_from
void parse_from(const rapidxml::xml_node< char > &node, COutputLogger &logger)
Definition: World_gui.cpp:41
check
ROSCPP_DECL bool check()
mvsim::WorldElementBase::factory
static Ptr factory(World *parent, const rapidxml::xml_node< char > *xml_node, const char *class_name=nullptr)
Definition: WorldElementBase.cpp:49
mvsim::parse_xmlnode_children_as_param
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const TParameterDefinitions &params, const std::map< std::string, std::string > &variableNamesValues={}, const char *functionNameContext="", mrpt::system::COutputLogger *logger=nullptr)
Definition: xml_utils.cpp:224
f
f
mvsim::World::setLightDirectionFromAzimuthElevation
void setLightDirectionFromAzimuthElevation(const float azimuth, const float elevation)
Definition: World_gui.cpp:1070
mvsim::World::GUI::InfoPerObject::cb
nanogui::CheckBox * cb
Definition: World.h:584
mvsim::World::internal_gui_on_image
mrpt::math::TPoint2D internal_gui_on_image(const std::string &label, const mrpt::img::CImage &im, int winPosX)
Definition: World_gui.cpp:985
mvsim::SensorBase::GetAllSensorsFOVViz
static std::shared_ptr< mrpt::opengl::CSetOfObjects > GetAllSensorsFOVViz()
Definition: SensorBase.cpp:65
mvsim::World::internalUpdate3DSceneObjects
void internalUpdate3DSceneObjects(mrpt::opengl::COpenGLScene &viz, mrpt::opengl::COpenGLScene &physical)
Definition: World_gui.cpp:771
s
XmlRpcServer s
mvsim::SensorBase::GetAllSensorsOriginViz
static std::shared_ptr< mrpt::opengl::CSetOfObjects > GetAllSensorsOriginViz()
Definition: SensorBase.cpp:59
mvsim::World::internal_gui_on_observation_3Dscan
void internal_gui_on_observation_3Dscan(const Simulable &veh, const std::shared_ptr< mrpt::obs::CObservation3DRangeScan > &obs)
Definition: World_gui.cpp:940
M_PI
#define M_PI
t
geometry_msgs::TransformStamped t
mvsim::World::GUI::InfoPerObject::simulable
Simulable::Ptr simulable
Definition: World.h:585
mvsim::World::TUpdateGUIParams::keyevent
TGUIKeyEvent keyevent
Keystrokes in the window are returned here.
Definition: World.h:163
mvsim::World::internal_GUI_thread
void internal_GUI_thread()
Definition: World_gui.cpp:444
mvsim::VisualObject::FreeOpenGLResources
static void FreeOpenGLResources()
Definition: VisualObject.cpp:154
mvsim::World::internal_gui_on_observation
void internal_gui_on_observation(const Simulable &veh, const mrpt::obs::CObservation::Ptr &obs)
Definition: World_gui.cpp:921
mvsim::World::TUpdateGUIParams::msg_lines
std::string msg_lines
Messages to show.
Definition: World.h:164
mvsim::VisualObject::showCollisionShape
void showCollisionShape(bool show)
Definition: VisualObject.cpp:246
ImGui::Button
IMGUI_API bool Button(const char *label, const ImVec2 &size=ImVec2(0, 0))
Definition: imgui_widgets.cpp:575
mvsim::World::is_GUI_open
bool is_GUI_open() const
Forces closing the GUI window, if any.
Definition: World_gui.cpp:50
mvsim::World::GUI::prepare_control_window
void prepare_control_window()
Definition: World_gui.cpp:55
xml_utils.h
mvsim::Simulable::getName
const std::string & getName() const
Definition: Simulable.h:92
mvsim::World::internalRunSensorsOn3DScene
void internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene &physicalObjects)
Definition: World_gui.cpp:757
action
action
mvsim::World::GUI::handle_mouse_operations
void handle_mouse_operations()
Definition: World_gui.cpp:676
mvsim::World::internal_gui_on_observation_image
void internal_gui_on_observation_image(const Simulable &veh, const std::shared_ptr< mrpt::obs::CObservationImage > &obs)
Definition: World_gui.cpp:971
mvsim::World::GUI::prepare_status_window
void prepare_status_window()
Definition: World_gui.cpp:169
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
rapidxml::xml_node< char >
mvsim::World::update_GUI
void update_GUI(TUpdateGUIParams *params=nullptr)
Definition: World_gui.cpp:855
mvsim::World::TGUI_Options::parse_from
void parse_from(const rapidxml::xml_node< char > &node, COutputLogger &logger)
Definition: World_gui.cpp:34
mvsim::World::internal_process_pending_gui_user_tasks
void internal_process_pending_gui_user_tasks()
Definition: World_gui.cpp:744
mvsim::World::internalGraphicsLoopTasksForSimulation
void internalGraphicsLoopTasksForSimulation()
Definition: World_gui.cpp:1036
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
mvsim::World::close_GUI
void close_GUI()
a previous call to update_GUI()
Definition: World_gui.cpp:52

mvsim
Author(s):
autogenerated on Tue Jul 4 2023 03:08:22