Debian Codegraph @

apps/holonomic-navigator-demo/holonomic_navigator_demoMain.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "holonomic_navigator_demoMain.h"
11
12 #include <mrpt/gui/about_box.h>
13 #include <wx/msgdlg.h>
14
15 //(*InternalHeaders(holonomic_navigator_demoFrame)
16 #include <wx/artprov.h>
17 #include <wx/bitmap.h>
18 #include <wx/font.h>
19 #include <wx/image.h>
20 #include <wx/intl.h>
21 #include <wx/settings.h>
22 #include <wx/string.h>
23 #include <wx/tglbtn.h>
24 //*)
25
26 #include <mrpt/gui/WxUtils.h>
27 #include <mrpt/io/CCompressedInputStream.h>
28 #include <mrpt/math/TLine3D.h>
29 #include <mrpt/math/TObject3D.h>
30 #include <mrpt/poses/CPoint2D.h>
31 #include <mrpt/poses/CPose2D.h>
32 #include <mrpt/serialization/CArchive.h>
33
34 #include "../ReactiveNavigationDemo/DEFAULT_GRIDMAP_DATA.h"
35 #include "../wx-common/mrpt_logo.xpm"
36 #include "imgs/main_icon.xpm"
37
38 // A custom Art provider for customizing the icons:
39 class MyArtProvider : public wxArtProvider
40 {
41 protected:
42 wxBitmap CreateBitmap(const wxArtID& id, const wxArtClient& client, const wxSize& size) override;
43 };
44
45 // CreateBitmap function
46 wxBitmap MyArtProvider::CreateBitmap(
47 const wxArtID& id, const wxArtClient& client, const wxSize& size)
48 {
49 if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(main_icon_xpm);
50 if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm);
51
52 // Any wxWidgets icons not implemented here
53 // will be provided by the default art provider.
54 return wxNullBitmap;
55 }
56
57 #include <mrpt/gui/about_box.h>
58 #include <mrpt/nav.h>
59 #include <mrpt/opengl.h>
60 #include <mrpt/system/filesystem.h>
61
62 #include <memory>
63
64 using namespace mrpt;
65 using namespace mrpt::maps;
66 using namespace mrpt::obs;
67 using namespace mrpt::io;
68 using namespace mrpt::img;
69 using namespace mrpt::opengl;
70 using namespace mrpt::serialization;
71 using namespace mrpt::math;
72 using namespace mrpt::config;
73 using namespace mrpt::system;
74 using namespace mrpt::nav;
75 using namespace mrpt::poses;
76 using namespace std;
77
78 //(*IdInit(holonomic_navigator_demoFrame)
79 const long holonomic_navigator_demoFrame::ID_BUTTON1 = wxNewId();
80 const long holonomic_navigator_demoFrame::ID_BUTTON2 = wxNewId();
81 const long holonomic_navigator_demoFrame::ID_BUTTON3 = wxNewId();
82 const long holonomic_navigator_demoFrame::ID_BUTTON6 = wxNewId();
83 const long holonomic_navigator_demoFrame::ID_BUTTON7 = wxNewId();
84 const long holonomic_navigator_demoFrame::ID_BUTTON4 = wxNewId();
85 const long holonomic_navigator_demoFrame::ID_BUTTON5 = wxNewId();
86 const long holonomic_navigator_demoFrame::ID_RADIOBOX1 = wxNewId();
87 const long holonomic_navigator_demoFrame::ID_TEXTCTRL1 = wxNewId();
88 const long holonomic_navigator_demoFrame::ID_PANEL1 = wxNewId();
89 const long holonomic_navigator_demoFrame::ID_PANEL2 = wxNewId();
90 const long holonomic_navigator_demoFrame::ID_NOTEBOOK1 = wxNewId();
91 const long holonomic_navigator_demoFrame::ID_STATICTEXT2 = wxNewId();
92 const long holonomic_navigator_demoFrame::ID_STATICTEXT1 = wxNewId();
93 const long holonomic_navigator_demoFrame::ID_XY_GLCANVAS = wxNewId();
94 const long holonomic_navigator_demoFrame::ID_CUSTOM1 = wxNewId();
95 const long holonomic_navigator_demoFrame::ID_TEXTCTRL2 = wxNewId();
96 const long holonomic_navigator_demoFrame::ID_MENUITEM4 = wxNewId();
97 const long holonomic_navigator_demoFrame::idMenuQuit = wxNewId();
98 const long holonomic_navigator_demoFrame::ID_MENUITEM1 = wxNewId();
99 const long holonomic_navigator_demoFrame::ID_MENUITEM2 = wxNewId();
100 const long holonomic_navigator_demoFrame::ID_MENUITEM3 = wxNewId();
101 const long holonomic_navigator_demoFrame::idMenuAbout = wxNewId();
102 const long holonomic_navigator_demoFrame::ID_STATUSBAR1 = wxNewId();
103 const long holonomic_navigator_demoFrame::ID_TIMER1 = wxNewId();
104 //*)
105
106 BEGIN_EVENT_TABLE(holonomic_navigator_demoFrame, wxFrame)
107 //(*EventTable(holonomic_navigator_demoFrame)
108 //*)
109 END_EVENT_TABLE()
110
111 holonomic_navigator_demoFrame::holonomic_navigator_demoFrame(wxWindow* parent, wxWindowID id) :
112 m_gridMap(),
113 m_targetPose(-5, -7, 0),
114 m_robotPose(0, 0, 0),
115 m_curCursorPos(0, 0),
116 m_cursorPickState(cpsNone)
117 {
118 // Load my custom icons:
119 wxArtProvider::Push(new MyArtProvider);
120
121 //(*Initialize(holonomic_navigator_demoFrame)
122 wxFlexGridSizer* FlexGridSizer4;
123 wxMenuItem* MenuItem2;
124 wxFlexGridSizer* FlexGridSizer3;
125 wxMenuItem* MenuItem1;
126 wxFlexGridSizer* FlexGridSizer5;
127 wxFlexGridSizer* FlexGridSizer2;
128 wxBoxSizer* BoxSizer2;
129 wxMenu* Menu1;
130 wxFlexGridSizer* FlexGridSizer7;
131 wxBoxSizer* BoxSizer1;
132 wxMenuBar* MenuBar1;
133 wxFlexGridSizer* FlexGridSizer1;
134 wxMenu* Menu2;
135
136 Create(
137 parent, id, _("Holonomic Navigation Demo - Part of MRPT"), wxDefaultPosition, wxDefaultSize,
138 wxDEFAULT_FRAME_STYLE, _T("id"));
139 FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0);
140 FlexGridSizer1->AddGrowableCol(0);
141 FlexGridSizer1->AddGrowableRow(1);
142 FlexGridSizer2 = new wxFlexGridSizer(1, 2, 0, 0);
143 FlexGridSizer2->AddGrowableCol(1);
144 FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
145 FlexGridSizer4->AddGrowableCol(0);
146 FlexGridSizer4->AddGrowableRow(0);
147 FlexGridSizer4->AddGrowableRow(1);
148 BoxSizer1 = new wxBoxSizer(wxHORIZONTAL);
149 btnLoadMap = new wxCustomButton(
150 this, ID_BUTTON1, _("Load map..."),
151 wxArtProvider::GetBitmap(
152 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FOLDER")),
153 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
154 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
155 _T("ID_BUTTON1"));
156 btnLoadMap->SetBitmapDisabled(btnLoadMap->CreateBitmapDisabled(btnLoadMap->GetBitmapLabel()));
157 btnLoadMap->SetBitmapMargin(wxSize(2, 4));
158 BoxSizer1->Add(btnLoadMap, 1, wxEXPAND, 1);
159 btnHelp = new wxCustomButton(
160 this, ID_BUTTON2, _("About..."),
161 wxArtProvider::GetBitmap(
162 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUESTION")),
163 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
164 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
165 _T("ID_BUTTON2"));
166 btnHelp->SetBitmapDisabled(btnHelp->CreateBitmapDisabled(btnHelp->GetBitmapLabel()));
167 btnHelp->SetBitmapMargin(wxSize(5, 4));
168 BoxSizer1->Add(btnHelp, 1, wxEXPAND, 1);
169 btnQuit = new wxCustomButton(
170 this, ID_BUTTON3, _("Exit"),
171 wxArtProvider::GetBitmap(
172 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUIT")),
173 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
174 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
175 _T("ID_BUTTON3"));
176 btnQuit->SetBitmapDisabled(btnQuit->CreateBitmapDisabled(btnQuit->GetBitmapLabel()));
177 BoxSizer1->Add(btnQuit, 1, wxEXPAND, 1);
178 FlexGridSizer4->Add(BoxSizer1, 1, wxEXPAND, 0);
179 BoxSizer2 = new wxBoxSizer(wxHORIZONTAL);
180 btnPlaceRobot = new wxCustomButton(
181 this, ID_BUTTON6, _("Place robot..."),
182 wxArtProvider::GetBitmap(
183 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
184 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
185 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
186 _T("ID_BUTTON6"));
187 btnPlaceRobot->SetBitmapDisabled(
188 btnPlaceRobot->CreateBitmapDisabled(btnPlaceRobot->GetBitmapLabel()));
189 btnPlaceRobot->SetBitmapMargin(wxSize(2, 4));
190 BoxSizer2->Add(btnPlaceRobot, 1, wxEXPAND, 2);
191 btnPlaceTarget = new wxCustomButton(
192 this, ID_BUTTON7, _("Set target..."),
193 wxArtProvider::GetBitmap(
194 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
195 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
196 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
197 _T("ID_BUTTON7"));
198 btnPlaceTarget->SetBitmapDisabled(
199 btnPlaceTarget->CreateBitmapDisabled(btnPlaceTarget->GetBitmapLabel()));
200 btnPlaceTarget->SetBitmapMargin(wxSize(2, 4));
201 BoxSizer2->Add(btnPlaceTarget, 1, wxEXPAND, 2);
202 btnStart = new wxCustomButton(
203 this, ID_BUTTON4, _("START"),
204 wxArtProvider::GetBitmap(
205 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")),
206 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
207 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
208 _T("ID_BUTTON4"));
209 btnStart->SetBitmapDisabled(btnStart->CreateBitmapDisabled(btnStart->GetBitmapLabel()));
210 btnStart->SetBitmapMargin(wxSize(2, 4));
211 BoxSizer2->Add(btnStart, 1, wxEXPAND, 2);
212 btnStop = new wxCustomButton(
213 this, ID_BUTTON5, _("STOP"),
214 wxArtProvider::GetBitmap(
215 wxART_MAKE_ART_ID_FROM_STR(_T("wxART_CROSS_MARK")),
216 wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
217 wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
218 _T("ID_BUTTON5"));
219 btnStop->SetBitmapDisabled(btnStop->CreateBitmapDisabled(btnStop->GetBitmapLabel()));
220 btnStop->SetBitmapMargin(wxSize(2, 4));
221 BoxSizer2->Add(btnStop, 1, wxEXPAND, 2);
222 FlexGridSizer4->Add(BoxSizer2, 1, wxEXPAND, 0);
223 FlexGridSizer2->Add(FlexGridSizer4, 1, wxEXPAND, 0);
224 Notebook1 =
225 new wxNotebook(this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_NOTEBOOK1"));
226 Panel1 = new wxPanel(
227 Notebook1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
228 FlexGridSizer7 = new wxFlexGridSizer(1, 2, 0, 0);
229 FlexGridSizer7->AddGrowableCol(1);
230 FlexGridSizer7->AddGrowableRow(0);
231 wxString __wxRadioBoxChoices_1[2] = {_("VFF (Virtual Force Field)"), _("ND (Nearness Diagram)")};
232 rbHoloMethod = new wxRadioBox(
233 Panel1, ID_RADIOBOX1, _(" Holonomic method "), wxDefaultPosition, wxDefaultSize, 2,
234 __wxRadioBoxChoices_1, 1, wxRA_HORIZONTAL, wxDefaultValidator, _T("ID_RADIOBOX1"));
235 FlexGridSizer7->Add(
236 rbHoloMethod, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
237 edHoloParams = new wxTextCtrl(
238 Panel1, ID_TEXTCTRL1, wxEmptyString, wxDefaultPosition, wxDefaultSize,
239 wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxVSCROLL | wxHSCROLL |
240 wxALWAYS_SHOW_SB,
241 wxDefaultValidator, _T("ID_TEXTCTRL1"));
242 edHoloParams->SetMinSize(wxSize(-1, 100));
243 wxFont edHoloParamsFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL);
244 edHoloParams->SetFont(edHoloParamsFont);
245 FlexGridSizer7->Add(edHoloParams, 1, wxEXPAND, 2);
246 Panel1->SetSizer(FlexGridSizer7);
247 FlexGridSizer7->Fit(Panel1);
248 FlexGridSizer7->SetSizeHints(Panel1);
249 Panel2 = new wxPanel(
250 Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2"));
251 Notebook1->AddPage(Panel1, _("Configuration"), true);
252 Notebook1->AddPage(Panel2, _("Stats"), false);
253 FlexGridSizer2->Add(Notebook1, 1, wxEXPAND, 2);
254 FlexGridSizer1->Add(FlexGridSizer2, 1, wxEXPAND, 0);
255 FlexGridSizer3 = new wxFlexGridSizer(2, 2, 0, 0);
256 FlexGridSizer3->AddGrowableCol(0);
257 FlexGridSizer3->AddGrowableCol(1);
258 FlexGridSizer3->AddGrowableRow(1);
259 StaticText2 = new wxStaticText(
260 this, ID_STATICTEXT2, _("[3D view of the simulated world]"), wxDefaultPosition, wxDefaultSize,
261 0, _T("ID_STATICTEXT2"));
262 FlexGridSizer3->Add(
263 StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
264 StaticText1 = new wxStaticText(
265 this, ID_STATICTEXT1, _("[Local view]"), wxDefaultPosition, wxDefaultSize, 0,
266 _T("ID_STATICTEXT1"));
267 FlexGridSizer3->Add(
268 StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
269 m_plot3D = new CMyGLCanvas(
270 this, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(450, 350), wxTAB_TRAVERSAL,
271 _T("ID_XY_GLCANVAS"));
272 FlexGridSizer3->Add(
273 m_plot3D, 1, wxALL | wxEXPAND | wxFIXED_MINSIZE | wxALIGN_LEFT | wxALIGN_TOP, 1);
274 FlexGridSizer5 = new wxFlexGridSizer(2, 1, 0, 0);
275 FlexGridSizer5->AddGrowableCol(0);
276 FlexGridSizer5->AddGrowableRow(0);
277 m_plotScan = new CMyGLCanvas(
278 this, ID_CUSTOM1, wxDefaultPosition, wxSize(150, 150), wxTAB_TRAVERSAL, _T("ID_CUSTOM1"));
279 FlexGridSizer5->Add(m_plotScan, 1, wxEXPAND, 1);
280 edInfoLocalView = new wxTextCtrl(
281 this, ID_TEXTCTRL2, wxEmptyString, wxDefaultPosition, wxDefaultSize,
282 wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator,
283 _T("ID_TEXTCTRL2"));
284 edInfoLocalView->SetMinSize(wxSize(-1, 50));
285 edInfoLocalView->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
286 wxFont edInfoLocalViewFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL);
287 edInfoLocalView->SetFont(edInfoLocalViewFont);
288 FlexGridSizer5->Add(edInfoLocalView, 1, wxEXPAND, 2);
289 FlexGridSizer3->Add(FlexGridSizer5, 1, wxEXPAND, 0);
290 FlexGridSizer1->Add(FlexGridSizer3, 1, wxEXPAND, 0);
291 SetSizer(FlexGridSizer1);
292 MenuBar1 = new wxMenuBar();
293 Menu1 = new wxMenu();
294 MenuItem3 = new wxMenuItem(Menu1, ID_MENUITEM4, _("Load map..."), wxEmptyString, wxITEM_NORMAL);
295 Menu1->Append(MenuItem3);
296 MenuItem1 = new wxMenuItem(
297 Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL);
298 Menu1->Append(MenuItem1);
299 MenuBar1->Append(Menu1, _("&File"));
300 Menu3 = new wxMenu();
301 mnuViewMaxRange =
302 new wxMenuItem(Menu3, ID_MENUITEM1, _("Maximum sensor range"), wxEmptyString, wxITEM_CHECK);
303 Menu3->Append(mnuViewMaxRange);
304 mnuViewRobotPath =
305 new wxMenuItem(Menu3, ID_MENUITEM2, _("Robot path"), wxEmptyString, wxITEM_CHECK);
306 Menu3->Append(mnuViewRobotPath);
307 Menu3->AppendSeparator();
308 MenuItem5 =
309 new wxMenuItem(Menu3, ID_MENUITEM3, _("Clear robot path"), wxEmptyString, wxITEM_NORMAL);
310 Menu3->Append(MenuItem5);
311 MenuBar1->Append(Menu3, _("&View"));
312 Menu2 = new wxMenu();
313 MenuItem2 = new wxMenuItem(
314 Menu2, idMenuAbout, _("About\tF1"), _("Show info about this application"), wxITEM_NORMAL);
315 Menu2->Append(MenuItem2);
316 MenuBar1->Append(Menu2, _("Help"));
317 SetMenuBar(MenuBar1);
318 StatusBar1 = new wxStatusBar(this, ID_STATUSBAR1, 0, _T("ID_STATUSBAR1"));
319 int __wxStatusBarWidths_1[3] = {-2, -2, -3};
320 int __wxStatusBarStyles_1[3] = {wxSB_NORMAL, wxSB_NORMAL, wxSB_NORMAL};
321 StatusBar1->SetFieldsCount(3, __wxStatusBarWidths_1);
322 StatusBar1->SetStatusStyles(3, __wxStatusBarStyles_1);
323 SetStatusBar(StatusBar1);
324 timRunSimul.SetOwner(this, ID_TIMER1);
325 timRunSimul.Start(10, false);
326 FlexGridSizer1->Fit(this);
327 FlexGridSizer1->SetSizeHints(this);
328 Center();
329
330 using hndf = holonomic_navigator_demoFrame;
331
332 Bind(wxEVT_BUTTON, &hndf::OnbtnLoadMapClick, this, ID_BUTTON1);
333 Bind(wxEVT_BUTTON, &hndf::OnAbout, this, ID_BUTTON2);
334 Bind(wxEVT_BUTTON, &hndf::OnQuit, this, ID_BUTTON3);
335 Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceRobotClick, this, ID_BUTTON6);
336 Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceTargetClick, this, ID_BUTTON7);
337 Bind(wxEVT_BUTTON, &hndf::OnbtnStartClick, this, ID_BUTTON4);
338 Bind(wxEVT_BUTTON, &hndf::OnbtnStopClick, this, ID_BUTTON5);
339 Bind(wxEVT_MENU, &hndf::OnbtnLoadMapClick, this, ID_MENUITEM4);
340 Bind(wxEVT_MENU, &hndf::OnQuit, this, idMenuQuit);
341 Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM1);
342 Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM2);
343 Bind(wxEVT_MENU, &hndf::OnMenuItemClearRobotPath, this, ID_MENUITEM3);
344 Bind(wxEVT_MENU, &hndf::OnAbout, this, idMenuAbout);
345 Bind(wxEVT_TIMER, &hndf::OntimRunSimulTrigger, this, ID_TIMER1);
346 //*)
347
348 m_plot3D->Bind(wxEVT_MOTION, &hndf::Onplot3DMouseMove, this);
349 m_plot3D->Bind(wxEVT_LEFT_DOWN, &hndf::Onplot3DMouseClick, this);
350
351 mnuViewMaxRange->Check(true);
352 mnuViewRobotPath->Check(true);
353
354 btnStart->Enable(true);
355 btnStop->Enable(false);
356
357 WX_START_TRY
358
359 // Initialize gridmap:
360 // -------------------------------
361 CMemoryStream s(DEFAULT_GRIDMAP_DATA, sizeof(DEFAULT_GRIDMAP_DATA));
362 archiveFrom(s) >> m_gridMap;
363
364 auto openGLSceneRef = m_plot3D->getOpenGLSceneRef();
365 // Populate 3D views:
366 // -------------------------------
367 {
368 mrpt::opengl::CGridPlaneXY::Ptr obj =
369 mrpt::opengl::CGridPlaneXY::Create(-50, 50, -50, 50, 0, 1);
370 obj->setColor_u8(TColor(30, 30, 30, 50));
371 openGLSceneRef->insert(obj);
372 }
373
374 gl_grid = mrpt::opengl::CSetOfObjects::Create();
375 openGLSceneRef->insert(gl_grid);
376 this->updateMap3DView();
377
378 gl_robot = mrpt::opengl::CSetOfObjects::Create();
379 {
380 mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f);
381 obj->setColor_u8(TColor::red());
382 gl_robot->insert(obj);
383 }
384 openGLSceneRef->insert(gl_robot);
385
386 gl_scan3D = mrpt::opengl::CPlanarLaserScan::Create();
387 gl_scan3D->enableLine(false);
388 gl_scan3D->setPointSize(2.0);
389 gl_scan3D->setLocation(0, 0, 0.01); // to avoid z-fighting in rendering
390 gl_robot->insert(gl_scan3D);
391
392 gl_robot_sensor_range = mrpt::opengl::CDisk::Create(0, 0);
393 gl_robot_sensor_range->setColor_u8(TColor(0, 0, 255, 90));
394 gl_robot_sensor_range->setLocation(0, 0, 0.01);
395 gl_robot->insert(gl_robot_sensor_range);
396
397 gl_robot_path = mrpt::opengl::CSetOfLines::Create();
398 gl_robot_path->setLineWidth(1);
399 gl_robot_path->setColor_u8(TColor(40, 40, 40, 200));
400 openGLSceneRef->insert(gl_robot_path);
401
402 gl_target = mrpt::opengl::CSetOfObjects::Create();
403 {
404 mrpt::opengl::CArrow::Ptr obj;
405 obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
406 obj->setColor_u8(TColor(0, 0, 255));
407 gl_target->insert(obj);
408 obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
409 obj->setColor_u8(TColor(0, 0, 255));
410 gl_target->insert(obj);
411 obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f);
412 obj->setColor_u8(TColor(0, 0, 255));
413 gl_target->insert(obj);
414 obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f);
415 obj->setColor_u8(TColor(0, 0, 255));
416 gl_target->insert(obj);
417 openGLSceneRef->insert(gl_target);
418 }
419
420 { // Sign of "picking a navigation target":
421 m_gl_placing_nav_target = std::make_shared<opengl::CSetOfObjects>();
422
423 mrpt::opengl::CArrow::Ptr obj;
424 obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
425 obj->setColor_u8(TColor(0, 0, 255));
426 m_gl_placing_nav_target->insert(obj);
427 obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
428 obj->setColor_u8(TColor(0, 0, 255));
429 m_gl_placing_nav_target->insert(obj);
430 obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f);
431 obj->setColor_u8(TColor(0, 0, 255));
432 m_gl_placing_nav_target->insert(obj);
433 obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f);
434 obj->setColor_u8(TColor(0, 0, 255));
435 m_gl_placing_nav_target->insert(obj);
436 m_gl_placing_nav_target->setVisibility(false); // Start invisible.
437 openGLSceneRef->insert(m_gl_placing_nav_target);
438 }
439 { // Sign of "replacing the robot":
440 m_gl_placing_robot = std::make_shared<opengl::CSetOfObjects>();
441 mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f);
442 obj->setColor_u8(TColor(255, 0, 0, 120));
443 m_gl_placing_robot->insert(obj);
444
445 m_gl_placing_robot->setVisibility(false); // Start invisible.
446 openGLSceneRef->insert(m_gl_placing_robot);
447 }
448
449 openGLSceneRef->insert(mrpt::opengl::stock_objects::CornerXYZ(1));
450
451 // Set camera:
452 m_plot3D->setCameraPointing(0.0f, 0.0f, 0.0f);
453 m_plot3D->setZoomDistance(40.0f);
454 m_plot3D->setElevationDegrees(70.0f);
455 m_plot3D->setAzimuthDegrees(-100.0f);
456 m_plot3D->setCameraProjective(true);
457
458 // 2D view ==============
459 auto openGLScanRef = m_plotScan->getOpenGLSceneRef();
460 {
461 mrpt::opengl::CGridPlaneXY::Ptr obj =
462 mrpt::opengl::CGridPlaneXY::Create(-1, 1.001f, -1, 1.001f, 0, 1);
463 obj->setColor_u8(TColor(30, 30, 30, 50));
464 openGLScanRef->insert(obj);
465 }
466
467 gl_scan2D = mrpt::opengl::CPlanarLaserScan::Create();
468 gl_scan2D->enableLine(false);
469 gl_scan2D->enableSurface(false);
470 gl_scan2D->setPointSize(2.0);
471 openGLScanRef->insert(gl_scan2D);
472
473 gl_line_direction = mrpt::opengl::CSimpleLine::Create();
474 gl_line_direction->setLineWidth(4);
475 gl_line_direction->setColor_u8(TColor(0, 0, 0));
476 openGLScanRef->insert(gl_line_direction);
477
478 gl_rel_target = mrpt::opengl::CPointCloud::Create();
479 gl_rel_target->setPointSize(7);
480 gl_rel_target->setColor_u8(TColor(0, 0, 255));
481 gl_rel_target->insertPoint(0, 0, 0);
482 openGLScanRef->insert(gl_rel_target);
483
484 openGLScanRef->insert(mrpt::opengl::stock_objects::CornerXYSimple(0.1f, 2));
485
486 gl_nd_gaps = mrpt::opengl::CSetOfLines::Create();
487 gl_nd_gaps->setLineWidth(2);
488 gl_nd_gaps->setColor_u8(TColor(204, 102, 51));
489 openGLScanRef->insert(gl_nd_gaps);
490
491 // Set camera:
492 m_plotScan->setCameraPointing(0.0f, 0.0f, 0.0f);
493 m_plotScan->setZoomDistance(2.2f);
494 m_plotScan->setElevationDegrees(90.0f);
495 m_plotScan->setAzimuthDegrees(-90.0f);
496 m_plotScan->setCameraProjective(false);
497
498 // Update positions of stuff:
499 this->updateViewsDynamicObjects();
500
501 // Retrieve default parameters for holonomic methods:
502 // ------------------------------------------------------
503 {
504 mrpt::config::CConfigFileMemory cfg;
505
506 m_simul_options.saveToConfigFile(cfg, "SIMULATOR");
507
508 mrpt::nav::CHolonomicVFF holo_VFF;
509 holo_VFF.options.saveToConfigFile(cfg, "VFF_CONFIG");
510
511 mrpt::nav::CHolonomicND holo_ND;
512 holo_ND.options.saveToConfigFile(cfg, "ND_CONFIG");
513
514 this->edHoloParams->SetValue(cfg.getContent().c_str());
515 }
516
517 WX_END_TRY
518
519 this->Maximize();
520 }
521
522 holonomic_navigator_demoFrame::~holonomic_navigator_demoFrame()
523 {
524 //(*Destroy(holonomic_navigator_demoFrame)
525 //*)
526 }
527
528 void holonomic_navigator_demoFrame::OnQuit(wxCommandEvent&) { Close(); }
529 void holonomic_navigator_demoFrame::OnAbout(wxCommandEvent&)
530 {
531 mrpt::gui::show_mrpt_about_box_wxWidgets(this, "holonomic-navigator-demo");
532 }
533
534 void holonomic_navigator_demoFrame::updateMap3DView() { m_gridMap.getVisualizationInto(*gl_grid); }
535
536 void holonomic_navigator_demoFrame::OnbtnPlaceRobotClick(wxCommandEvent& event)
537 {
538 if (m_cursorPickState != cpsPlaceRobot)
539 {
540 m_cursorPickState = cpsPlaceRobot;
541 m_plot3D->SetCursor(*wxCROSS_CURSOR);
542 }
543 else
544 { // Cancel:
545 m_cursorPickState = cpsNone;
546 m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
547 m_gl_placing_robot->setVisibility(false);
548 }
549 btnPlaceRobot->SetValue(m_cursorPickState == cpsPlaceRobot);
550 btnPlaceRobot->Refresh();
551 }
552
553 void holonomic_navigator_demoFrame::OnbtnPlaceTargetClick(wxCommandEvent& event)
554 {
555 if (m_cursorPickState != cpsPickTarget)
556 {
557 m_cursorPickState = cpsPickTarget;
558 m_plot3D->SetCursor(*wxCROSS_CURSOR);
559 }
560 else
561 { // Cancel:
562 m_cursorPickState = cpsNone;
563 m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
564 m_gl_placing_nav_target->setVisibility(false);
565 }
566 btnPlaceTarget->SetValue(m_cursorPickState == cpsPickTarget);
567 btnPlaceTarget->Refresh();
568 }
569
570 void holonomic_navigator_demoFrame::OnbtnStartClick(wxCommandEvent& event)
571 {
572 reinitSimulator();
573
574 btnStart->Enable(false);
575 btnStart->Refresh();
576 btnStop->Enable(true);
577 btnStop->Refresh();
578 edHoloParams->Enable(false);
579 rbHoloMethod->Enable(false);
580 }
581
582 void holonomic_navigator_demoFrame::OnbtnStopClick(wxCommandEvent& event)
583 {
584 btnStart->Enable(true);
585 btnStart->Refresh();
586 btnStop->Enable(false);
587 btnStop->Refresh();
588 edHoloParams->Enable(true);
589 rbHoloMethod->Enable(true);
590 }
591
592 // Run simulator (when "running"):
593 void holonomic_navigator_demoFrame::OntimRunSimulTrigger(wxTimerEvent& event)
594 {
595 try
596 {
597 if (btnStop->IsEnabled())
598 {
599 simulateOneStep(event.GetInterval() * 1e-3);
600 }
601 updateViewsDynamicObjects();
602 }
603 catch (const std::exception& e)
604 {
605 wxMessageBox(mrpt::exception_to_str(e), wxT("Exception"), wxOK, this);
606 // Stop:
607 wxCommandEvent ev;
608 OnbtnStopClick(ev);
609 }
610 }
611
612 // Create navigator object & load params from GUI:
613 void holonomic_navigator_demoFrame::reinitSimulator()
614 {
615 WX_START_TRY
616
617 // Delete old & build new navigator:
618 m_holonomicMethod.reset();
619 switch (rbHoloMethod->GetSelection())
620 {
621 case 0:
622 m_holonomicMethod = std::make_unique<mrpt::nav::CHolonomicVFF>();
623 break;
624 case 1:
625 m_holonomicMethod = std::make_unique<mrpt::nav::CHolonomicND>();
626 break;
627 default:
628 throw std::runtime_error("Invalid holonomic method selected!");
629 };
630
631 // Load params:
632 {
633 CConfigFileMemory cfg;
634 cfg.setContent(std::string(edHoloParams->GetValue().mb_str()));
635 m_holonomicMethod->initialize(cfg);
636
637 m_simul_options.loadFromConfigFile(cfg, "SIMULATOR");
638 }
639
640 // Update GUI stuff:
641 gl_robot_sensor_range->setDiskRadius(
642 m_simul_options.MAX_SENSOR_RADIUS * 1.01, m_simul_options.MAX_SENSOR_RADIUS * 0.99);
643
644 WX_END_TRY
645 }
646
647 void holonomic_navigator_demoFrame::simulateOneStep(double time_step)
648 {
649 // Simulate 360deg range scan:
650 CObservation2DRangeScan simulatedScan;
651
652 simulatedScan.aperture = M_2PIf;
653 simulatedScan.rightToLeft = true;
654 simulatedScan.maxRange = m_simul_options.MAX_SENSOR_RADIUS;
655 simulatedScan.sensorPose = CPose3D();
656
657 m_gridMap.laserScanSimulator(
658 simulatedScan, mrpt::poses::CPose2D(m_robotPose), 0.5, m_simul_options.SENSOR_NUM_RANGES,
659 m_simul_options.SENSOR_RANGE_NOISE_STD);
660
661 gl_scan3D->setScan(simulatedScan); // Draw real scan in 3D view
662
663 // Normalize:
664 for (size_t j = 0; j < simulatedScan.getScanSize(); j++)
665 simulatedScan.setScanRange(j, simulatedScan.getScanRange(j) / simulatedScan.maxRange);
666
667 gl_scan2D->setScan(simulatedScan); // Draw scaled scan in right-hand view
668
669 // Navigate:
670 mrpt::math::TPose2D relTargetPose = m_targetPose - m_robotPose;
671 // Normalized relative target:
672 relTargetPose.x *= 1.0 / simulatedScan.maxRange;
673 relTargetPose.y *= 1.0 / simulatedScan.maxRange;
674
675 // tictac.Tic();
676 CAbstractHolonomicReactiveMethod::NavOutput no;
677 CAbstractHolonomicReactiveMethod::NavInput ni;
678 ni.targets.push_back(relTargetPose);
679
680 ni.obstacles.resize(simulatedScan.getScanSize());
681 for (unsigned int i = 0; i < ni.obstacles.size(); i++)
682 ni.obstacles[i] = simulatedScan.getScanRange(i);
683
684 ni.maxRobotSpeed = m_simul_options.ROBOT_MAX_SPEED;
685 ni.maxObstacleDist = m_simul_options.MAX_SENSOR_RADIUS;
686
687 this->m_holonomicMethod->navigate(ni, no);
688
689 mrpt::nav::CHolonomicLogFileRecord::Ptr out_log = no.logRecord;
690
691 // Move robot:
692 m_robotPose.x += cos(no.desiredDirection) * no.desiredSpeed * time_step;
693 m_robotPose.y += sin(no.desiredDirection) * no.desiredSpeed * time_step;
694
695 // Update path graph:
696 const TPoint3D cur_pt(m_robotPose.x, m_robotPose.y, 0.01);
697
698 if (gl_robot_path->empty())
699 gl_robot_path->appendLine(cur_pt, cur_pt);
700 else
701 gl_robot_path->appendLineStrip(cur_pt);
702
703 gl_rel_target->setLocation(relTargetPose.x, relTargetPose.y, 0);
704
705 // Clear stuff which will be updated if used below:
706 edInfoLocalView->Clear();
707 gl_nd_gaps->clear();
708
709 // Update 2D view graphs:
710 if (out_log && IS_CLASS(*out_log, CLogFileRecord_ND))
711 {
712 CLogFileRecord_ND::Ptr log = std::dynamic_pointer_cast<CLogFileRecord_ND>(out_log);
713 const size_t nGaps = log->gaps_ini.size();
714
715 const string sSitu =
716 mrpt::typemeta::TEnumType<CHolonomicND::TSituations>::value2name(log->situation);
717
718 string sLog = mrpt::format("ND situation : %s\n", sSitu.c_str());
719 sLog += mrpt::format("Gap count : %u\n", static_cast<unsigned int>(nGaps));
720
721 edInfoLocalView->SetValue(sLog.c_str());
722
723 gl_nd_gaps->appendLine(0, 0, 0, 0, 0, 0);
724
725 for (size_t i = 0; i < nGaps; i++)
726 {
727 const size_t N_STEPS = 20;
728 for (size_t j = 0; j < N_STEPS; j++)
729 {
730 const double sec = log->gaps_ini[i] + j * (log->gaps_end[i] - log->gaps_ini[i]) /
731 static_cast<double>(N_STEPS - 1);
732 const double ang = M_PI * (-1 + 2 * sec / ((float)simulatedScan.getScanSize()));
733 const double d = simulatedScan.getScanRange(sec) - 0.05;
734 gl_nd_gaps->appendLineStrip(d * cos(ang), d * sin(ang), 0);
735 }
736 gl_nd_gaps->appendLineStrip(0, 0, 0);
737 }
738 }
739
740 // Movement direction:
741 const double d = no.desiredSpeed / m_simul_options.ROBOT_MAX_SPEED;
742 gl_line_direction->setLineCoords(
743 0, 0, 0, cos(no.desiredDirection) * d, sin(no.desiredDirection) * d, 0);
744 }
745
746 void holonomic_navigator_demoFrame::updateViewsDynamicObjects()
747 {
748 gl_robot->setLocation(m_robotPose.x, m_robotPose.y, 0);
749
750 // animate target:
751 {
752 const double TARGET_BOUNCE_MIN = 0.7;
753 const double TARGET_BOUNCE_MAX = 1;
754
755 const double TARGET_BOUNCE_PERIOD = 1.0;
756 const double t = fmod(m_runtime.Tac(), TARGET_BOUNCE_PERIOD) / TARGET_BOUNCE_PERIOD;
757
758 // Parabolic path
759 const double s = 4 * t * (TARGET_BOUNCE_MAX - TARGET_BOUNCE_MIN) * (1 - t) + TARGET_BOUNCE_MIN;
760
761 gl_target->setLocation(mrpt::math::TPoint3D(m_targetPose.translation()));
762 gl_target->setScale(s);
763 }
764
765 // Labels:
766 StatusBar1->SetStatusText(mrpt::format("Robot: (%.03f,%.03f)", m_robotPose.x, m_robotPose.y), 0);
767 StatusBar1->SetStatusText(mrpt::format("Target: %s", m_targetPose.asString().c_str()));
768
769 // Show/hide:
770 gl_robot_sensor_range->setVisibility(mnuViewMaxRange->IsChecked());
771 gl_robot_path->setVisibility(mnuViewRobotPath->IsChecked());
772
773 // Refresh:
774 m_plot3D->Refresh();
775 m_plotScan->Refresh();
776 }
777
778 void holonomic_navigator_demoFrame::Onplot3DMouseMove(wxMouseEvent& event)
779 {
780 int X, Y;
781 event.GetPosition(&X, &Y);
782
783 // Intersection of 3D ray with ground plane ====================
784 TLine3D ray;
785 m_plot3D->getOpenGLSceneRef()->getViewport("main")->get3DRayForPixelCoord(X, Y, ray);
786 // Create a 3D plane, e.g. Z=0
787 const TPlane ground_plane(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0));
788 // Intersection of the line with the plane:
789 TObject3D inters;
790 intersect(ray, ground_plane, inters);
791 // Interpret the intersection as a point, if there is an intersection:
792 TPoint3D inters_pt;
793 if (inters.getPoint(inters_pt))
794 {
795 m_curCursorPos.x = inters_pt.x;
796 m_curCursorPos.y = inters_pt.y;
797
798 if (m_cursorPickState == cpsPickTarget)
799 {
800 m_gl_placing_nav_target->setVisibility(true);
801 m_gl_placing_nav_target->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05);
802 }
803 else if (m_cursorPickState == cpsPlaceRobot)
804 {
805 m_gl_placing_robot->setVisibility(true);
806 m_gl_placing_robot->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05);
807 }
808
809 StatusBar1->SetStatusText(
810 wxString::Format(wxT("X=%.03f Y=%.04f Z=0"), m_curCursorPos.x, m_curCursorPos.y), 2);
811 }
812
813 // Do normal process in that class:
814 m_plot3D->OnMouseMove(event);
815 }
816
817 void holonomic_navigator_demoFrame::Onplot3DMouseClick(wxMouseEvent& event)
818 {
819 switch (m_cursorPickState)
820 {
821 case cpsPickTarget:
822 {
823 m_targetPose = {m_curCursorPos.x, m_curCursorPos.y, .0};
824
825 btnPlaceTarget->SetValue(false);
826 btnPlaceTarget->Refresh();
827 m_gl_placing_nav_target->setVisibility(false);
828 break;
829 }
830 case cpsPlaceRobot:
831 {
832 m_robotPose.x = m_curCursorPos.x;
833 m_robotPose.y = m_curCursorPos.y;
834
835 btnPlaceRobot->SetValue(false);
836 btnPlaceRobot->Refresh();
837 m_gl_placing_robot->setVisibility(false);
838 break;
839 }
840 default:
841 break;
842 }
843
844 m_plot3D->SetCursor(*wxSTANDARD_CURSOR); // End of cross cursor
845 m_cursorPickState = cpsNone; // end of mode
846
847 // Do normal process in that class:
848 m_plot3D->OnMouseDown(event);
849 }
850
851 // ==== holonomic_navigator_demoFrame::TOptions ======
852 void holonomic_navigator_demoFrame::TOptions::loadFromConfigFile(
853 const mrpt::config::CConfigFileBase& source, const std::string& section)
854 {
855 MRPT_START
856
857 // Load from config text:
858 MRPT_LOAD_CONFIG_VAR(ROBOT_MAX_SPEED, double, source, section);
859 MRPT_LOAD_CONFIG_VAR(MAX_SENSOR_RADIUS, double, source, section);
860 MRPT_LOAD_CONFIG_VAR(SENSOR_NUM_RANGES, uint64_t, source, section);
861 MRPT_LOAD_CONFIG_VAR(SENSOR_RANGE_NOISE_STD, double, source, section);
862
863 MRPT_END
864 }
865
866 void holonomic_navigator_demoFrame::TOptions::saveToConfigFile(
867 mrpt::config::CConfigFileBase& cfg, const std::string& section) const
868 {
869 MRPT_START
870 const int WN = 40, WV = 20;
871
872 cfg.write(
873 section, "ROBOT_MAX_SPEED", ROBOT_MAX_SPEED, WN, WV, "Maximum speed for the robot (m/s)");
874 cfg.write(
875 section, "MAX_SENSOR_RADIUS", MAX_SENSOR_RADIUS, WN, WV,
876 "Maximum range of the 360deg sensor (meters)");
877 cfg.write(
878 section, "SENSOR_NUM_RANGES", SENSOR_NUM_RANGES, WN, WV,
879 "Number of ranges in the 360deg sensor FOV");
880 cfg.write(
881 section, "SENSOR_RANGE_NOISE_STD", SENSOR_RANGE_NOISE_STD, WN, WV,
882 "Sensor noise (one sigma, in meters)");
883
884 MRPT_END
885 }
886
887 void holonomic_navigator_demoFrame::OnMenuItemChangeVisibleStuff(wxCommandEvent& event)
888 {
889 updateViewsDynamicObjects();
890 }
891
892 void holonomic_navigator_demoFrame::OnMenuItemClearRobotPath(wxCommandEvent& event)
893 {
894 gl_robot_path->clear();
895 updateViewsDynamicObjects();
896 }
897
898 void holonomic_navigator_demoFrame::OnbtnLoadMapClick(wxCommandEvent& event)
899 {
900 WX_START_TRY
901
902 wxFileDialog dlg(
903 this, _("Select grid map to load"), _("."), _("grid.png"),
904 wxT("Image files (*.png,*.jpg,*.gif)|*.png;*.jpg;*.gif|Binary gridmap "
905 "files (*.gridmap,*.gridmap.gz)|*.gridmap;*.gridmap.gz|All files "
906 "(*.*)|*.*"),
907 wxFD_OPEN | wxFD_FILE_MUST_EXIST);
908
909 if (dlg.ShowModal() != wxID_OK) return;
910
911 const wxString sFil = dlg.GetPath();
912 const std::string fil = std::string(sFil.mb_str());
913
914 const std::string fil_ext = mrpt::system::extractFileExtension(fil, true);
915
916 if (mrpt::system::lowerCase(fil_ext) == "gridmap")
917 {
918 CCompressedInputStream f(fil);
919 archiveFrom(f) >> m_gridMap;
920 }
921 else
922 {
923 // Try loading the image:
924 CImage img;
925 if (!img.loadFromFile(fil, 0 /* force grayscale */))
926 {
927 wxMessageBox(_("Error"), _("Can't load the image file (check its format)."));
928 }
929 else
930 {
931 // We also need the size of each pixel:
932 double cx = -1;
933 double cy = -1;
934 double cell_size = 0.05;
935
936 const wxString sCellSize = wxGetTextFromUser(
937 _("Enter the size (in meters) of each pixel:"), _("Grid parameters"), _("0.05"), this);
938 const wxString sCX = wxGetTextFromUser(
939 _("Enter the central pixel (x-coordinate), or -1 = the image "
940 "center:"),
941 _("Grid parameters"), _("-1"), this);
942 const wxString sCY = wxGetTextFromUser(
943 _("Enter the central pixel (y-coordinate), or -1 = the image "
944 "center:"),
945 _("Grid parameters"), _("-1"), this);
946
947 if (sCellSize.ToCDouble(&cell_size) && sCX.ToCDouble(&cx) && sCY.ToCDouble(&cy))
948 {
949 if (!m_gridMap.loadFromBitmap(img, cell_size, {cx, cy}))
950 wxMessageBox(_("Error"), _("Can't load the image file into the gridmap..."));
951 }
952 else
953 wxMessageBox(_("Error"), _("Error parsing the numbers you entered..."));
954 }
955 }
956
957 updateMap3DView();
958 m_plot3D->Refresh();
959
960 WX_END_TRY
961 }