/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "holonomic_navigator_demoMain.h" #include #include //(*InternalHeaders(holonomic_navigator_demoFrame) #include #include #include #include #include #include #include #include //*) #include #include #include #include #include #include #include #include "../ReactiveNavigationDemo/DEFAULT_GRIDMAP_DATA.h" #include "../wx-common/mrpt_logo.xpm" #include "imgs/main_icon.xpm" // A custom Art provider for customizing the icons: class MyArtProvider : public wxArtProvider { protected: wxBitmap CreateBitmap(const wxArtID& id, const wxArtClient& client, const wxSize& size) override; }; // CreateBitmap function wxBitmap MyArtProvider::CreateBitmap( const wxArtID& id, const wxArtClient& client, const wxSize& size) { if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(main_icon_xpm); if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm); // Any wxWidgets icons not implemented here // will be provided by the default art provider. return wxNullBitmap; } #include #include #include #include #include using namespace mrpt; using namespace mrpt::maps; using namespace mrpt::obs; using namespace mrpt::io; using namespace mrpt::img; using namespace mrpt::opengl; using namespace mrpt::serialization; using namespace mrpt::math; using namespace mrpt::config; using namespace mrpt::system; using namespace mrpt::nav; using namespace mrpt::poses; using namespace std; //(*IdInit(holonomic_navigator_demoFrame) const long holonomic_navigator_demoFrame::ID_BUTTON1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON2 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON3 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON6 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON7 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON4 = wxNewId(); const long holonomic_navigator_demoFrame::ID_BUTTON5 = wxNewId(); const long holonomic_navigator_demoFrame::ID_RADIOBOX1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_TEXTCTRL1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_PANEL1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_PANEL2 = wxNewId(); const long holonomic_navigator_demoFrame::ID_NOTEBOOK1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_STATICTEXT2 = wxNewId(); const long holonomic_navigator_demoFrame::ID_STATICTEXT1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_XY_GLCANVAS = wxNewId(); const long holonomic_navigator_demoFrame::ID_CUSTOM1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_TEXTCTRL2 = wxNewId(); const long holonomic_navigator_demoFrame::ID_MENUITEM4 = wxNewId(); const long holonomic_navigator_demoFrame::idMenuQuit = wxNewId(); const long holonomic_navigator_demoFrame::ID_MENUITEM1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_MENUITEM2 = wxNewId(); const long holonomic_navigator_demoFrame::ID_MENUITEM3 = wxNewId(); const long holonomic_navigator_demoFrame::idMenuAbout = wxNewId(); const long holonomic_navigator_demoFrame::ID_STATUSBAR1 = wxNewId(); const long holonomic_navigator_demoFrame::ID_TIMER1 = wxNewId(); //*) BEGIN_EVENT_TABLE(holonomic_navigator_demoFrame, wxFrame) //(*EventTable(holonomic_navigator_demoFrame) //*) END_EVENT_TABLE() holonomic_navigator_demoFrame::holonomic_navigator_demoFrame(wxWindow* parent, wxWindowID id) : m_gridMap(), m_targetPose(-5, -7, 0), m_robotPose(0, 0, 0), m_curCursorPos(0, 0), m_cursorPickState(cpsNone) { // Load my custom icons: wxArtProvider::Push(new MyArtProvider); //(*Initialize(holonomic_navigator_demoFrame) wxFlexGridSizer* FlexGridSizer4; wxMenuItem* MenuItem2; wxFlexGridSizer* FlexGridSizer3; wxMenuItem* MenuItem1; wxFlexGridSizer* FlexGridSizer5; wxFlexGridSizer* FlexGridSizer2; wxBoxSizer* BoxSizer2; wxMenu* Menu1; wxFlexGridSizer* FlexGridSizer7; wxBoxSizer* BoxSizer1; wxMenuBar* MenuBar1; wxFlexGridSizer* FlexGridSizer1; wxMenu* Menu2; Create( parent, id, _("Holonomic Navigation Demo - Part of MRPT"), wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("id")); FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0); FlexGridSizer1->AddGrowableCol(0); FlexGridSizer1->AddGrowableRow(1); FlexGridSizer2 = new wxFlexGridSizer(1, 2, 0, 0); FlexGridSizer2->AddGrowableCol(1); FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0); FlexGridSizer4->AddGrowableCol(0); FlexGridSizer4->AddGrowableRow(0); FlexGridSizer4->AddGrowableRow(1); BoxSizer1 = new wxBoxSizer(wxHORIZONTAL); btnLoadMap = new wxCustomButton( this, ID_BUTTON1, _("Load map..."), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FOLDER")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON1")); btnLoadMap->SetBitmapDisabled(btnLoadMap->CreateBitmapDisabled(btnLoadMap->GetBitmapLabel())); btnLoadMap->SetBitmapMargin(wxSize(2, 4)); BoxSizer1->Add(btnLoadMap, 1, wxEXPAND, 1); btnHelp = new wxCustomButton( this, ID_BUTTON2, _("About..."), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUESTION")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON2")); btnHelp->SetBitmapDisabled(btnHelp->CreateBitmapDisabled(btnHelp->GetBitmapLabel())); btnHelp->SetBitmapMargin(wxSize(5, 4)); BoxSizer1->Add(btnHelp, 1, wxEXPAND, 1); btnQuit = new wxCustomButton( this, ID_BUTTON3, _("Exit"), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUIT")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON3")); btnQuit->SetBitmapDisabled(btnQuit->CreateBitmapDisabled(btnQuit->GetBitmapLabel())); BoxSizer1->Add(btnQuit, 1, wxEXPAND, 1); FlexGridSizer4->Add(BoxSizer1, 1, wxEXPAND, 0); BoxSizer2 = new wxBoxSizer(wxHORIZONTAL); btnPlaceRobot = new wxCustomButton( this, ID_BUTTON6, _("Place robot..."), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON6")); btnPlaceRobot->SetBitmapDisabled( btnPlaceRobot->CreateBitmapDisabled(btnPlaceRobot->GetBitmapLabel())); btnPlaceRobot->SetBitmapMargin(wxSize(2, 4)); BoxSizer2->Add(btnPlaceRobot, 1, wxEXPAND, 2); btnPlaceTarget = new wxCustomButton( this, ID_BUTTON7, _("Set target..."), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON7")); btnPlaceTarget->SetBitmapDisabled( btnPlaceTarget->CreateBitmapDisabled(btnPlaceTarget->GetBitmapLabel())); btnPlaceTarget->SetBitmapMargin(wxSize(2, 4)); BoxSizer2->Add(btnPlaceTarget, 1, wxEXPAND, 2); btnStart = new wxCustomButton( this, ID_BUTTON4, _("START"), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON4")); btnStart->SetBitmapDisabled(btnStart->CreateBitmapDisabled(btnStart->GetBitmapLabel())); btnStart->SetBitmapMargin(wxSize(2, 4)); BoxSizer2->Add(btnStart, 1, wxEXPAND, 2); btnStop = new wxCustomButton( this, ID_BUTTON5, _("STOP"), wxArtProvider::GetBitmap( wxART_MAKE_ART_ID_FROM_STR(_T("wxART_CROSS_MARK")), wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))), wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator, _T("ID_BUTTON5")); btnStop->SetBitmapDisabled(btnStop->CreateBitmapDisabled(btnStop->GetBitmapLabel())); btnStop->SetBitmapMargin(wxSize(2, 4)); BoxSizer2->Add(btnStop, 1, wxEXPAND, 2); FlexGridSizer4->Add(BoxSizer2, 1, wxEXPAND, 0); FlexGridSizer2->Add(FlexGridSizer4, 1, wxEXPAND, 0); Notebook1 = new wxNotebook(this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_NOTEBOOK1")); Panel1 = new wxPanel( Notebook1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1")); FlexGridSizer7 = new wxFlexGridSizer(1, 2, 0, 0); FlexGridSizer7->AddGrowableCol(1); FlexGridSizer7->AddGrowableRow(0); wxString __wxRadioBoxChoices_1[2] = {_("VFF (Virtual Force Field)"), _("ND (Nearness Diagram)")}; rbHoloMethod = new wxRadioBox( Panel1, ID_RADIOBOX1, _(" Holonomic method "), wxDefaultPosition, wxDefaultSize, 2, __wxRadioBoxChoices_1, 1, wxRA_HORIZONTAL, wxDefaultValidator, _T("ID_RADIOBOX1")); FlexGridSizer7->Add( rbHoloMethod, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5); edHoloParams = new wxTextCtrl( Panel1, ID_TEXTCTRL1, wxEmptyString, wxDefaultPosition, wxDefaultSize, wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxVSCROLL | wxHSCROLL | wxALWAYS_SHOW_SB, wxDefaultValidator, _T("ID_TEXTCTRL1")); edHoloParams->SetMinSize(wxSize(-1, 100)); wxFont edHoloParamsFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL); edHoloParams->SetFont(edHoloParamsFont); FlexGridSizer7->Add(edHoloParams, 1, wxEXPAND, 2); Panel1->SetSizer(FlexGridSizer7); FlexGridSizer7->Fit(Panel1); FlexGridSizer7->SetSizeHints(Panel1); Panel2 = new wxPanel( Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2")); Notebook1->AddPage(Panel1, _("Configuration"), true); Notebook1->AddPage(Panel2, _("Stats"), false); FlexGridSizer2->Add(Notebook1, 1, wxEXPAND, 2); FlexGridSizer1->Add(FlexGridSizer2, 1, wxEXPAND, 0); FlexGridSizer3 = new wxFlexGridSizer(2, 2, 0, 0); FlexGridSizer3->AddGrowableCol(0); FlexGridSizer3->AddGrowableCol(1); FlexGridSizer3->AddGrowableRow(1); StaticText2 = new wxStaticText( this, ID_STATICTEXT2, _("[3D view of the simulated world]"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT2")); FlexGridSizer3->Add( StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5); StaticText1 = new wxStaticText( this, ID_STATICTEXT1, _("[Local view]"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT1")); FlexGridSizer3->Add( StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5); m_plot3D = new CMyGLCanvas( this, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(450, 350), wxTAB_TRAVERSAL, _T("ID_XY_GLCANVAS")); FlexGridSizer3->Add( m_plot3D, 1, wxALL | wxEXPAND | wxFIXED_MINSIZE | wxALIGN_LEFT | wxALIGN_TOP, 1); FlexGridSizer5 = new wxFlexGridSizer(2, 1, 0, 0); FlexGridSizer5->AddGrowableCol(0); FlexGridSizer5->AddGrowableRow(0); m_plotScan = new CMyGLCanvas( this, ID_CUSTOM1, wxDefaultPosition, wxSize(150, 150), wxTAB_TRAVERSAL, _T("ID_CUSTOM1")); FlexGridSizer5->Add(m_plotScan, 1, wxEXPAND, 1); edInfoLocalView = new wxTextCtrl( this, ID_TEXTCTRL2, wxEmptyString, wxDefaultPosition, wxDefaultSize, wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator, _T("ID_TEXTCTRL2")); edInfoLocalView->SetMinSize(wxSize(-1, 50)); edInfoLocalView->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE)); wxFont edInfoLocalViewFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL); edInfoLocalView->SetFont(edInfoLocalViewFont); FlexGridSizer5->Add(edInfoLocalView, 1, wxEXPAND, 2); FlexGridSizer3->Add(FlexGridSizer5, 1, wxEXPAND, 0); FlexGridSizer1->Add(FlexGridSizer3, 1, wxEXPAND, 0); SetSizer(FlexGridSizer1); MenuBar1 = new wxMenuBar(); Menu1 = new wxMenu(); MenuItem3 = new wxMenuItem(Menu1, ID_MENUITEM4, _("Load map..."), wxEmptyString, wxITEM_NORMAL); Menu1->Append(MenuItem3); MenuItem1 = new wxMenuItem( Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL); Menu1->Append(MenuItem1); MenuBar1->Append(Menu1, _("&File")); Menu3 = new wxMenu(); mnuViewMaxRange = new wxMenuItem(Menu3, ID_MENUITEM1, _("Maximum sensor range"), wxEmptyString, wxITEM_CHECK); Menu3->Append(mnuViewMaxRange); mnuViewRobotPath = new wxMenuItem(Menu3, ID_MENUITEM2, _("Robot path"), wxEmptyString, wxITEM_CHECK); Menu3->Append(mnuViewRobotPath); Menu3->AppendSeparator(); MenuItem5 = new wxMenuItem(Menu3, ID_MENUITEM3, _("Clear robot path"), wxEmptyString, wxITEM_NORMAL); Menu3->Append(MenuItem5); MenuBar1->Append(Menu3, _("&View")); Menu2 = new wxMenu(); MenuItem2 = new wxMenuItem( Menu2, idMenuAbout, _("About\tF1"), _("Show info about this application"), wxITEM_NORMAL); Menu2->Append(MenuItem2); MenuBar1->Append(Menu2, _("Help")); SetMenuBar(MenuBar1); StatusBar1 = new wxStatusBar(this, ID_STATUSBAR1, 0, _T("ID_STATUSBAR1")); int __wxStatusBarWidths_1[3] = {-2, -2, -3}; int __wxStatusBarStyles_1[3] = {wxSB_NORMAL, wxSB_NORMAL, wxSB_NORMAL}; StatusBar1->SetFieldsCount(3, __wxStatusBarWidths_1); StatusBar1->SetStatusStyles(3, __wxStatusBarStyles_1); SetStatusBar(StatusBar1); timRunSimul.SetOwner(this, ID_TIMER1); timRunSimul.Start(10, false); FlexGridSizer1->Fit(this); FlexGridSizer1->SetSizeHints(this); Center(); using hndf = holonomic_navigator_demoFrame; Bind(wxEVT_BUTTON, &hndf::OnbtnLoadMapClick, this, ID_BUTTON1); Bind(wxEVT_BUTTON, &hndf::OnAbout, this, ID_BUTTON2); Bind(wxEVT_BUTTON, &hndf::OnQuit, this, ID_BUTTON3); Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceRobotClick, this, ID_BUTTON6); Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceTargetClick, this, ID_BUTTON7); Bind(wxEVT_BUTTON, &hndf::OnbtnStartClick, this, ID_BUTTON4); Bind(wxEVT_BUTTON, &hndf::OnbtnStopClick, this, ID_BUTTON5); Bind(wxEVT_MENU, &hndf::OnbtnLoadMapClick, this, ID_MENUITEM4); Bind(wxEVT_MENU, &hndf::OnQuit, this, idMenuQuit); Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM1); Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM2); Bind(wxEVT_MENU, &hndf::OnMenuItemClearRobotPath, this, ID_MENUITEM3); Bind(wxEVT_MENU, &hndf::OnAbout, this, idMenuAbout); Bind(wxEVT_TIMER, &hndf::OntimRunSimulTrigger, this, ID_TIMER1); //*) m_plot3D->Bind(wxEVT_MOTION, &hndf::Onplot3DMouseMove, this); m_plot3D->Bind(wxEVT_LEFT_DOWN, &hndf::Onplot3DMouseClick, this); mnuViewMaxRange->Check(true); mnuViewRobotPath->Check(true); btnStart->Enable(true); btnStop->Enable(false); WX_START_TRY // Initialize gridmap: // ------------------------------- CMemoryStream s(DEFAULT_GRIDMAP_DATA, sizeof(DEFAULT_GRIDMAP_DATA)); archiveFrom(s) >> m_gridMap; auto openGLSceneRef = m_plot3D->getOpenGLSceneRef(); // Populate 3D views: // ------------------------------- { mrpt::opengl::CGridPlaneXY::Ptr obj = mrpt::opengl::CGridPlaneXY::Create(-50, 50, -50, 50, 0, 1); obj->setColor_u8(TColor(30, 30, 30, 50)); openGLSceneRef->insert(obj); } gl_grid = mrpt::opengl::CSetOfObjects::Create(); openGLSceneRef->insert(gl_grid); this->updateMap3DView(); gl_robot = mrpt::opengl::CSetOfObjects::Create(); { mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f); obj->setColor_u8(TColor::red()); gl_robot->insert(obj); } openGLSceneRef->insert(gl_robot); gl_scan3D = mrpt::opengl::CPlanarLaserScan::Create(); gl_scan3D->enableLine(false); gl_scan3D->setPointSize(2.0); gl_scan3D->setLocation(0, 0, 0.01); // to avoid z-fighting in rendering gl_robot->insert(gl_scan3D); gl_robot_sensor_range = mrpt::opengl::CDisk::Create(0, 0); gl_robot_sensor_range->setColor_u8(TColor(0, 0, 255, 90)); gl_robot_sensor_range->setLocation(0, 0, 0.01); gl_robot->insert(gl_robot_sensor_range); gl_robot_path = mrpt::opengl::CSetOfLines::Create(); gl_robot_path->setLineWidth(1); gl_robot_path->setColor_u8(TColor(40, 40, 40, 200)); openGLSceneRef->insert(gl_robot_path); gl_target = mrpt::opengl::CSetOfObjects::Create(); { mrpt::opengl::CArrow::Ptr obj; obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); gl_target->insert(obj); obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); gl_target->insert(obj); obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); gl_target->insert(obj); obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); gl_target->insert(obj); openGLSceneRef->insert(gl_target); } { // Sign of "picking a navigation target": m_gl_placing_nav_target = std::make_shared(); mrpt::opengl::CArrow::Ptr obj; obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); m_gl_placing_nav_target->insert(obj); obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); m_gl_placing_nav_target->insert(obj); obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); m_gl_placing_nav_target->insert(obj); obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f); obj->setColor_u8(TColor(0, 0, 255)); m_gl_placing_nav_target->insert(obj); m_gl_placing_nav_target->setVisibility(false); // Start invisible. openGLSceneRef->insert(m_gl_placing_nav_target); } { // Sign of "replacing the robot": m_gl_placing_robot = std::make_shared(); mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f); obj->setColor_u8(TColor(255, 0, 0, 120)); m_gl_placing_robot->insert(obj); m_gl_placing_robot->setVisibility(false); // Start invisible. openGLSceneRef->insert(m_gl_placing_robot); } openGLSceneRef->insert(mrpt::opengl::stock_objects::CornerXYZ(1)); // Set camera: m_plot3D->setCameraPointing(0.0f, 0.0f, 0.0f); m_plot3D->setZoomDistance(40.0f); m_plot3D->setElevationDegrees(70.0f); m_plot3D->setAzimuthDegrees(-100.0f); m_plot3D->setCameraProjective(true); // 2D view ============== auto openGLScanRef = m_plotScan->getOpenGLSceneRef(); { mrpt::opengl::CGridPlaneXY::Ptr obj = mrpt::opengl::CGridPlaneXY::Create(-1, 1.001f, -1, 1.001f, 0, 1); obj->setColor_u8(TColor(30, 30, 30, 50)); openGLScanRef->insert(obj); } gl_scan2D = mrpt::opengl::CPlanarLaserScan::Create(); gl_scan2D->enableLine(false); gl_scan2D->enableSurface(false); gl_scan2D->setPointSize(2.0); openGLScanRef->insert(gl_scan2D); gl_line_direction = mrpt::opengl::CSimpleLine::Create(); gl_line_direction->setLineWidth(4); gl_line_direction->setColor_u8(TColor(0, 0, 0)); openGLScanRef->insert(gl_line_direction); gl_rel_target = mrpt::opengl::CPointCloud::Create(); gl_rel_target->setPointSize(7); gl_rel_target->setColor_u8(TColor(0, 0, 255)); gl_rel_target->insertPoint(0, 0, 0); openGLScanRef->insert(gl_rel_target); openGLScanRef->insert(mrpt::opengl::stock_objects::CornerXYSimple(0.1f, 2)); gl_nd_gaps = mrpt::opengl::CSetOfLines::Create(); gl_nd_gaps->setLineWidth(2); gl_nd_gaps->setColor_u8(TColor(204, 102, 51)); openGLScanRef->insert(gl_nd_gaps); // Set camera: m_plotScan->setCameraPointing(0.0f, 0.0f, 0.0f); m_plotScan->setZoomDistance(2.2f); m_plotScan->setElevationDegrees(90.0f); m_plotScan->setAzimuthDegrees(-90.0f); m_plotScan->setCameraProjective(false); // Update positions of stuff: this->updateViewsDynamicObjects(); // Retrieve default parameters for holonomic methods: // ------------------------------------------------------ { mrpt::config::CConfigFileMemory cfg; m_simul_options.saveToConfigFile(cfg, "SIMULATOR"); mrpt::nav::CHolonomicVFF holo_VFF; holo_VFF.options.saveToConfigFile(cfg, "VFF_CONFIG"); mrpt::nav::CHolonomicND holo_ND; holo_ND.options.saveToConfigFile(cfg, "ND_CONFIG"); this->edHoloParams->SetValue(cfg.getContent().c_str()); } WX_END_TRY this->Maximize(); } holonomic_navigator_demoFrame::~holonomic_navigator_demoFrame() { //(*Destroy(holonomic_navigator_demoFrame) //*) } void holonomic_navigator_demoFrame::OnQuit(wxCommandEvent&) { Close(); } void holonomic_navigator_demoFrame::OnAbout(wxCommandEvent&) { mrpt::gui::show_mrpt_about_box_wxWidgets(this, "holonomic-navigator-demo"); } void holonomic_navigator_demoFrame::updateMap3DView() { m_gridMap.getVisualizationInto(*gl_grid); } void holonomic_navigator_demoFrame::OnbtnPlaceRobotClick(wxCommandEvent& event) { if (m_cursorPickState != cpsPlaceRobot) { m_cursorPickState = cpsPlaceRobot; m_plot3D->SetCursor(*wxCROSS_CURSOR); } else { // Cancel: m_cursorPickState = cpsNone; m_plot3D->SetCursor(*wxSTANDARD_CURSOR); m_gl_placing_robot->setVisibility(false); } btnPlaceRobot->SetValue(m_cursorPickState == cpsPlaceRobot); btnPlaceRobot->Refresh(); } void holonomic_navigator_demoFrame::OnbtnPlaceTargetClick(wxCommandEvent& event) { if (m_cursorPickState != cpsPickTarget) { m_cursorPickState = cpsPickTarget; m_plot3D->SetCursor(*wxCROSS_CURSOR); } else { // Cancel: m_cursorPickState = cpsNone; m_plot3D->SetCursor(*wxSTANDARD_CURSOR); m_gl_placing_nav_target->setVisibility(false); } btnPlaceTarget->SetValue(m_cursorPickState == cpsPickTarget); btnPlaceTarget->Refresh(); } void holonomic_navigator_demoFrame::OnbtnStartClick(wxCommandEvent& event) { reinitSimulator(); btnStart->Enable(false); btnStart->Refresh(); btnStop->Enable(true); btnStop->Refresh(); edHoloParams->Enable(false); rbHoloMethod->Enable(false); } void holonomic_navigator_demoFrame::OnbtnStopClick(wxCommandEvent& event) { btnStart->Enable(true); btnStart->Refresh(); btnStop->Enable(false); btnStop->Refresh(); edHoloParams->Enable(true); rbHoloMethod->Enable(true); } // Run simulator (when "running"): void holonomic_navigator_demoFrame::OntimRunSimulTrigger(wxTimerEvent& event) { try { if (btnStop->IsEnabled()) { simulateOneStep(event.GetInterval() * 1e-3); } updateViewsDynamicObjects(); } catch (const std::exception& e) { wxMessageBox(mrpt::exception_to_str(e), wxT("Exception"), wxOK, this); // Stop: wxCommandEvent ev; OnbtnStopClick(ev); } } // Create navigator object & load params from GUI: void holonomic_navigator_demoFrame::reinitSimulator() { WX_START_TRY // Delete old & build new navigator: m_holonomicMethod.reset(); switch (rbHoloMethod->GetSelection()) { case 0: m_holonomicMethod = std::make_unique(); break; case 1: m_holonomicMethod = std::make_unique(); break; default: throw std::runtime_error("Invalid holonomic method selected!"); }; // Load params: { CConfigFileMemory cfg; cfg.setContent(std::string(edHoloParams->GetValue().mb_str())); m_holonomicMethod->initialize(cfg); m_simul_options.loadFromConfigFile(cfg, "SIMULATOR"); } // Update GUI stuff: gl_robot_sensor_range->setDiskRadius( m_simul_options.MAX_SENSOR_RADIUS * 1.01, m_simul_options.MAX_SENSOR_RADIUS * 0.99); WX_END_TRY } void holonomic_navigator_demoFrame::simulateOneStep(double time_step) { // Simulate 360deg range scan: CObservation2DRangeScan simulatedScan; simulatedScan.aperture = M_2PIf; simulatedScan.rightToLeft = true; simulatedScan.maxRange = m_simul_options.MAX_SENSOR_RADIUS; simulatedScan.sensorPose = CPose3D(); m_gridMap.laserScanSimulator( simulatedScan, mrpt::poses::CPose2D(m_robotPose), 0.5, m_simul_options.SENSOR_NUM_RANGES, m_simul_options.SENSOR_RANGE_NOISE_STD); gl_scan3D->setScan(simulatedScan); // Draw real scan in 3D view // Normalize: for (size_t j = 0; j < simulatedScan.getScanSize(); j++) simulatedScan.setScanRange(j, simulatedScan.getScanRange(j) / simulatedScan.maxRange); gl_scan2D->setScan(simulatedScan); // Draw scaled scan in right-hand view // Navigate: mrpt::math::TPose2D relTargetPose = m_targetPose - m_robotPose; // Normalized relative target: relTargetPose.x *= 1.0 / simulatedScan.maxRange; relTargetPose.y *= 1.0 / simulatedScan.maxRange; // tictac.Tic(); CAbstractHolonomicReactiveMethod::NavOutput no; CAbstractHolonomicReactiveMethod::NavInput ni; ni.targets.push_back(relTargetPose); ni.obstacles.resize(simulatedScan.getScanSize()); for (unsigned int i = 0; i < ni.obstacles.size(); i++) ni.obstacles[i] = simulatedScan.getScanRange(i); ni.maxRobotSpeed = m_simul_options.ROBOT_MAX_SPEED; ni.maxObstacleDist = m_simul_options.MAX_SENSOR_RADIUS; this->m_holonomicMethod->navigate(ni, no); mrpt::nav::CHolonomicLogFileRecord::Ptr out_log = no.logRecord; // Move robot: m_robotPose.x += cos(no.desiredDirection) * no.desiredSpeed * time_step; m_robotPose.y += sin(no.desiredDirection) * no.desiredSpeed * time_step; // Update path graph: const TPoint3D cur_pt(m_robotPose.x, m_robotPose.y, 0.01); if (gl_robot_path->empty()) gl_robot_path->appendLine(cur_pt, cur_pt); else gl_robot_path->appendLineStrip(cur_pt); gl_rel_target->setLocation(relTargetPose.x, relTargetPose.y, 0); // Clear stuff which will be updated if used below: edInfoLocalView->Clear(); gl_nd_gaps->clear(); // Update 2D view graphs: if (out_log && IS_CLASS(*out_log, CLogFileRecord_ND)) { CLogFileRecord_ND::Ptr log = std::dynamic_pointer_cast(out_log); const size_t nGaps = log->gaps_ini.size(); const string sSitu = mrpt::typemeta::TEnumType::value2name(log->situation); string sLog = mrpt::format("ND situation : %s\n", sSitu.c_str()); sLog += mrpt::format("Gap count : %u\n", static_cast(nGaps)); edInfoLocalView->SetValue(sLog.c_str()); gl_nd_gaps->appendLine(0, 0, 0, 0, 0, 0); for (size_t i = 0; i < nGaps; i++) { const size_t N_STEPS = 20; for (size_t j = 0; j < N_STEPS; j++) { const double sec = log->gaps_ini[i] + j * (log->gaps_end[i] - log->gaps_ini[i]) / static_cast(N_STEPS - 1); const double ang = M_PI * (-1 + 2 * sec / ((float)simulatedScan.getScanSize())); const double d = simulatedScan.getScanRange(sec) - 0.05; gl_nd_gaps->appendLineStrip(d * cos(ang), d * sin(ang), 0); } gl_nd_gaps->appendLineStrip(0, 0, 0); } } // Movement direction: const double d = no.desiredSpeed / m_simul_options.ROBOT_MAX_SPEED; gl_line_direction->setLineCoords( 0, 0, 0, cos(no.desiredDirection) * d, sin(no.desiredDirection) * d, 0); } void holonomic_navigator_demoFrame::updateViewsDynamicObjects() { gl_robot->setLocation(m_robotPose.x, m_robotPose.y, 0); // animate target: { const double TARGET_BOUNCE_MIN = 0.7; const double TARGET_BOUNCE_MAX = 1; const double TARGET_BOUNCE_PERIOD = 1.0; const double t = fmod(m_runtime.Tac(), TARGET_BOUNCE_PERIOD) / TARGET_BOUNCE_PERIOD; // Parabolic path const double s = 4 * t * (TARGET_BOUNCE_MAX - TARGET_BOUNCE_MIN) * (1 - t) + TARGET_BOUNCE_MIN; gl_target->setLocation(mrpt::math::TPoint3D(m_targetPose.translation())); gl_target->setScale(s); } // Labels: StatusBar1->SetStatusText(mrpt::format("Robot: (%.03f,%.03f)", m_robotPose.x, m_robotPose.y), 0); StatusBar1->SetStatusText(mrpt::format("Target: %s", m_targetPose.asString().c_str())); // Show/hide: gl_robot_sensor_range->setVisibility(mnuViewMaxRange->IsChecked()); gl_robot_path->setVisibility(mnuViewRobotPath->IsChecked()); // Refresh: m_plot3D->Refresh(); m_plotScan->Refresh(); } void holonomic_navigator_demoFrame::Onplot3DMouseMove(wxMouseEvent& event) { int X, Y; event.GetPosition(&X, &Y); // Intersection of 3D ray with ground plane ==================== TLine3D ray; m_plot3D->getOpenGLSceneRef()->getViewport("main")->get3DRayForPixelCoord(X, Y, ray); // Create a 3D plane, e.g. Z=0 const TPlane ground_plane(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0)); // Intersection of the line with the plane: TObject3D inters; intersect(ray, ground_plane, inters); // Interpret the intersection as a point, if there is an intersection: TPoint3D inters_pt; if (inters.getPoint(inters_pt)) { m_curCursorPos.x = inters_pt.x; m_curCursorPos.y = inters_pt.y; if (m_cursorPickState == cpsPickTarget) { m_gl_placing_nav_target->setVisibility(true); m_gl_placing_nav_target->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05); } else if (m_cursorPickState == cpsPlaceRobot) { m_gl_placing_robot->setVisibility(true); m_gl_placing_robot->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05); } StatusBar1->SetStatusText( wxString::Format(wxT("X=%.03f Y=%.04f Z=0"), m_curCursorPos.x, m_curCursorPos.y), 2); } // Do normal process in that class: m_plot3D->OnMouseMove(event); } void holonomic_navigator_demoFrame::Onplot3DMouseClick(wxMouseEvent& event) { switch (m_cursorPickState) { case cpsPickTarget: { m_targetPose = {m_curCursorPos.x, m_curCursorPos.y, .0}; btnPlaceTarget->SetValue(false); btnPlaceTarget->Refresh(); m_gl_placing_nav_target->setVisibility(false); break; } case cpsPlaceRobot: { m_robotPose.x = m_curCursorPos.x; m_robotPose.y = m_curCursorPos.y; btnPlaceRobot->SetValue(false); btnPlaceRobot->Refresh(); m_gl_placing_robot->setVisibility(false); break; } default: break; } m_plot3D->SetCursor(*wxSTANDARD_CURSOR); // End of cross cursor m_cursorPickState = cpsNone; // end of mode // Do normal process in that class: m_plot3D->OnMouseDown(event); } // ==== holonomic_navigator_demoFrame::TOptions ====== void holonomic_navigator_demoFrame::TOptions::loadFromConfigFile( const mrpt::config::CConfigFileBase& source, const std::string& section) { MRPT_START // Load from config text: MRPT_LOAD_CONFIG_VAR(ROBOT_MAX_SPEED, double, source, section); MRPT_LOAD_CONFIG_VAR(MAX_SENSOR_RADIUS, double, source, section); MRPT_LOAD_CONFIG_VAR(SENSOR_NUM_RANGES, uint64_t, source, section); MRPT_LOAD_CONFIG_VAR(SENSOR_RANGE_NOISE_STD, double, source, section); MRPT_END } void holonomic_navigator_demoFrame::TOptions::saveToConfigFile( mrpt::config::CConfigFileBase& cfg, const std::string& section) const { MRPT_START const int WN = 40, WV = 20; cfg.write( section, "ROBOT_MAX_SPEED", ROBOT_MAX_SPEED, WN, WV, "Maximum speed for the robot (m/s)"); cfg.write( section, "MAX_SENSOR_RADIUS", MAX_SENSOR_RADIUS, WN, WV, "Maximum range of the 360deg sensor (meters)"); cfg.write( section, "SENSOR_NUM_RANGES", SENSOR_NUM_RANGES, WN, WV, "Number of ranges in the 360deg sensor FOV"); cfg.write( section, "SENSOR_RANGE_NOISE_STD", SENSOR_RANGE_NOISE_STD, WN, WV, "Sensor noise (one sigma, in meters)"); MRPT_END } void holonomic_navigator_demoFrame::OnMenuItemChangeVisibleStuff(wxCommandEvent& event) { updateViewsDynamicObjects(); } void holonomic_navigator_demoFrame::OnMenuItemClearRobotPath(wxCommandEvent& event) { gl_robot_path->clear(); updateViewsDynamicObjects(); } void holonomic_navigator_demoFrame::OnbtnLoadMapClick(wxCommandEvent& event) { WX_START_TRY wxFileDialog dlg( this, _("Select grid map to load"), _("."), _("grid.png"), wxT("Image files (*.png,*.jpg,*.gif)|*.png;*.jpg;*.gif|Binary gridmap " "files (*.gridmap,*.gridmap.gz)|*.gridmap;*.gridmap.gz|All files " "(*.*)|*.*"), wxFD_OPEN | wxFD_FILE_MUST_EXIST); if (dlg.ShowModal() != wxID_OK) return; const wxString sFil = dlg.GetPath(); const std::string fil = std::string(sFil.mb_str()); const std::string fil_ext = mrpt::system::extractFileExtension(fil, true); if (mrpt::system::lowerCase(fil_ext) == "gridmap") { CCompressedInputStream f(fil); archiveFrom(f) >> m_gridMap; } else { // Try loading the image: CImage img; if (!img.loadFromFile(fil, 0 /* force grayscale */)) { wxMessageBox(_("Error"), _("Can't load the image file (check its format).")); } else { // We also need the size of each pixel: double cx = -1; double cy = -1; double cell_size = 0.05; const wxString sCellSize = wxGetTextFromUser( _("Enter the size (in meters) of each pixel:"), _("Grid parameters"), _("0.05"), this); const wxString sCX = wxGetTextFromUser( _("Enter the central pixel (x-coordinate), or -1 = the image " "center:"), _("Grid parameters"), _("-1"), this); const wxString sCY = wxGetTextFromUser( _("Enter the central pixel (y-coordinate), or -1 = the image " "center:"), _("Grid parameters"), _("-1"), this); if (sCellSize.ToCDouble(&cell_size) && sCX.ToCDouble(&cx) && sCY.ToCDouble(&cy)) { if (!m_gridMap.loadFromBitmap(img, cell_size, {cx, cy})) wxMessageBox(_("Error"), _("Can't load the image file into the gridmap...")); } else wxMessageBox(_("Error"), _("Error parsing the numbers you entered...")); } } updateMap3DView(); m_plot3D->Refresh(); WX_END_TRY }