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


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21