// ------------------------------------------------------------------------ // Generate a MATLAB script that draws the overall navigation log // ------------------------------------------------------------------------ void navlog_viewer_GUI_designDialog::OnmnuMatlabPlotsSelected(wxCommandEvent& event) { WX_START_TRY wxFileDialog dialog( this, _("Save MATLAB/Octave script that draws the log...") /*caption*/, _(".") /* defaultDir */, _("drawLog.m") /* defaultFilename */, _("MATLAB/Octave script (*.m)|*.m") /* wildcard */, wxFD_SAVE | wxFD_OVERWRITE_PROMPT ); if (dialog.ShowModal() != wxID_OK) return; const string filName(dialog.GetPath().mb_str()); ofstream f(filName.c_str()); if (!f.is_open()) throw runtime_error("Error writing to file!"); f << "% Script for drawing navigation log\n" << "% Generated automatically by navlog-viewer - MRPT " << mrpt::system::MRPT_getVersion() << "\n" << "% From log: " << string(edLogFile->GetValue().mbc_str()) << "\n" << "% -------------------------------------------------------------------------\n\n"; f << "%%\n" << "function [] = main()\n" << "% Robot shape: (x,y) in each line\n" << "rs = [-0.3 -0.3;0.6 -0.3;0.6 0.3;-0.3 0.3];\n" << "decimate_robot_shapes = 15;" << "decim_cnt=0;"; const int DECIMATE_POINTS = 10; int decim_point_cnt =0; std::vector<float> X,Y; // Obstacles std::vector<float> TX,TY; // Target over time const size_t N = m_logdata.size(); for (size_t i=0;i<N;i++) { const CLogFileRecordPtr logsptr = CLogFileRecordPtr( m_logdata[i] ); const CLogFileRecord * logptr = logsptr.pointer(); f << format("decim_cnt=decim_cnt+1; if (decim_cnt>=decimate_robot_shapes); drawRobotShape(rs,[%f %f %f]); decim_cnt=0; end\n", logptr->robotOdometryPose.x(), logptr->robotOdometryPose.y(), logptr->robotOdometryPose.phi() ); if (++decim_point_cnt>=DECIMATE_POINTS) { CSimplePointsMap pts; pts.changeCoordinatesReference( logptr->WS_Obstacles, logptr->robotOdometryPose ); const std::vector<float> &pX = pts.getPointsBufferRef_x(); const std::vector<float> &pY = pts.getPointsBufferRef_y(); X.insert(X.begin(),pX.begin(),pX.end()); Y.insert(Y.begin(),pY.begin(),pY.end()); } // Target: const mrpt::math::TPoint2D trg_glob = mrpt::math::TPoint2D( logptr->robotOdometryPose+logptr->WS_target_relative ); if (TX.empty() || std::abs((*TX.rbegin())-trg_glob.x)>1e-3 || std::abs((*TY.rbegin())-trg_glob.y)>1e-3 ) { TX.push_back(trg_glob.x); TY.push_back(trg_glob.y); } } f << "\n % Points: \n" << " Ps = ["; for (size_t k=0;k<X.size();k++) f << X[k] << " " << Y[k] << "\n"; f << "];\n" << "plot(Ps(:,1),Ps(:,2),'k.','MarkerSize',3);\n"; f << "\n % Target point: \n" << " Ts = ["; for (size_t k=0;k<TX.size();k++) f << TX[k] << " " << TY[k] << "\n"; f << "];\n" << "plot(Ts(:,1),Ts(:,2),'rx','MarkerSize',5);\n"; f << "axis equal;\n" << "\n"; f << "%% drawRobotShape()\n" << "function [] = drawRobotShape(sh,pose)\n" << "nPts=size(sh,1);\n" << "Pts=[];\n" << "for i=1:(nPts+1),\n" << " j=mod(i-1,nPts)+1;\n" << " cc=cos(pose(3)); ss=sin(pose(3)); x=pose(1); y=pose(2);\n" << " Pts=[Pts;x+cc*sh(j,1)-ss*sh(j,2) y+ss*sh(j,1)+cc*sh(j,2) ];\n" << "end\n" << "plot(Pts(:,1),Pts(:,2)); hold on;\n"; WX_END_TRY }
uint32_t CSimplePointsMap_getSize(CSimplePointsMap& self) { return self.getPointsBufferRef_x().size(); }