| 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 |
} |