qslamandnavwidget.cpp
Go to the documentation of this file.
00001 #include "qslamandnavwidget.h"
00002 #include <stdio.h>
00003 using namespace GMapping;
00004 
00005 
00006 QSLAMandNavWidget::QSLAMandNavWidget( QWidget * parent, const char * name, WFlags f)
00007 : QMapPainter(parent, name, f), dumper("slamandnav", 1){
00008         robotPose=IntPoint(0,0);
00009         robotHeading=0;
00010         slamRestart=false;
00011         slamFinished=false;
00012         enableMotion=false;
00013         startWalker=false;
00014         trajectorySent=false;
00015         goHome=false;
00016         wantsQuit=false;
00017         printHelp=false;
00018         saveGoalPoints=false;
00019         writeImages=false;
00020         drawRobot=true;
00021 }
00022 
00023 QSLAMandNavWidget::~QSLAMandNavWidget(){}
00024 
00025 
00026 void QSLAMandNavWidget::mousePressEvent ( QMouseEvent * e ){
00027         QPoint p=e->pos();
00028         int mx=p.x();
00029         int my=height()-p.y();
00030         if ( e->state()&Qt::ShiftButton && e->button()==Qt::LeftButton) {
00031                 if (trajectorySent)
00032                         trajectoryPoints.clear();
00033                 e->accept();
00034                 IntPoint p=IntPoint(mx, my);
00035                 trajectoryPoints.push_back(p);
00036                 trajectorySent=false;
00037         } 
00038 }
00039 
00040 void QSLAMandNavWidget::keyPressEvent ( QKeyEvent * e ){
00041         if (e->key()==Qt::Key_Delete){
00042                 e->accept();
00043                 if (!trajectoryPoints.empty())
00044                         trajectoryPoints.pop_back();
00045         }
00046         if (e->key()==Qt::Key_S){
00047                 e->accept();
00048                 enableMotion=!enableMotion;
00049         }
00050         if (e->key()==Qt::Key_W){
00051                 e->accept();
00052                 startWalker=!startWalker;
00053         }
00054         if (e->key()==Qt::Key_G){
00055                 e->accept();
00056                 slamRestart=true;
00057         }
00058         if (e->key()==Qt::Key_T){
00059                 e->accept();
00060                 trajectorySent=true;
00061         }
00062         if (e->key()==Qt::Key_R){
00063                 e->accept();
00064                 goHome=true;
00065         }       
00066         if (e->key()==Qt::Key_C){
00067                 e->accept();
00068                 slamFinished=true;
00069                 
00070         }
00071         if (e->key()==Qt::Key_Q){
00072                 e->accept();
00073                 wantsQuit=true;
00074                 
00075         }
00076         if (e->key()==Qt::Key_H){
00077                 e->accept();
00078                 printHelp=true;
00079                 //BABSI
00080                 //insert the help here
00081         }
00082         if (e->key()==Qt::Key_Y){
00083                 e->accept();
00084                 saveGoalPoints=true;
00085                 //BABSI
00086                 //insert the help here
00087         }
00088         if (e->key()==Qt::Key_D){
00089                 e->accept();
00090                 drawRobot=!drawRobot;
00091                 //BABSI
00092                 //insert the help here
00093         }
00094 }
00095 
00096 void QSLAMandNavWidget::paintEvent ( QPaintEvent * ){
00097         QPixmap pixmap(*m_pixmap);
00098         QPainter painter(&pixmap);
00099         if (trajectorySent)
00100         painter.setPen(Qt::red);
00101         bool first=true;
00102         int oldx=0, oldy=0;
00103         //paint the path
00104         for (std::list<IntPoint>::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){
00105                 int x=it->x;
00106                 int y=height()-it->y;
00107                 if (! first)
00108                         painter.drawLine(oldx, oldy, x,y);
00109                 oldx=x;
00110                 oldy=y;
00111                 first=false;
00112         }
00113 
00114         //paint the robot
00115         if (drawRobot){
00116                 painter.setPen(Qt::black);
00117                 int rx=robotPose.x;
00118                 int ry=height()-robotPose.y;
00119                 int robotSize=6;
00120                 painter.drawLine(rx, ry, 
00121                                 rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading)));
00122                 painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize);
00123         }
00124         if (writeImages){
00125                 dumper.dump(pixmap);
00126         }
00127         bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP);
00128 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13