qnavigatorwidget.cpp
Go to the documentation of this file.
2 #include <stdio.h>
3 using namespace GMapping;
4 
5 
6 QNavigatorWidget::QNavigatorWidget( QWidget * parent, const char * name, WFlags f)
7 : QMapPainter(parent, name, f), dumper("navigator", 1){
8  robotPose=IntPoint(0,0);
9  robotHeading=0;
10  confirmLocalization=false;
11  repositionRobot=false;
12  startWalker=false;
13  enableMotion=false;
14  goHome=false;
15  trajectorySent=false;
16  writeImages=false;
17  drawRobot=true;
18  wantsQuit=false;
19 }
20 
22 
23 
24 void QNavigatorWidget::mousePressEvent ( QMouseEvent * e ){
25  QPoint p=e->pos();
26  int mx=p.x();
27  int my=height()-p.y();
28  if (!(e->state()&Qt::ShiftButton) && e->button()==Qt::LeftButton) {
29  if (trajectorySent)
30  trajectoryPoints.clear();
31  e->accept();
32  IntPoint p=IntPoint(mx, my);
33  trajectoryPoints.push_back(p);
34  trajectorySent=false;
35  }
36  if (e->state()&Qt::ControlButton && e->button()==Qt::LeftButton){
37  e->accept();
38  robotPose=IntPoint(mx, my);
39  repositionRobot=true;
41  }
42  if (e->state()&Qt::ControlButton && e->button()==Qt::RightButton){
43  e->accept();
44  IntPoint p(mx, my);
45  p=p-robotPose;
46  robotHeading=atan2(p.y, p.x);
47  repositionRobot=true;
49  }
50 }
51 
52 void QNavigatorWidget::keyPressEvent ( QKeyEvent * e ){
53  if (e->key()==Qt::Key_Delete){
54  e->accept();
55  if (!trajectoryPoints.empty())
56  trajectoryPoints.pop_back();
57  }
58  if (e->key()==Qt::Key_S){
59  e->accept();
61  }
62  if (e->key()==Qt::Key_W){
63  e->accept();
65  }
66  if (e->key()==Qt::Key_G){
67  e->accept();
69  }
70  if (e->key()==Qt::Key_T){
71  e->accept();
72  trajectorySent=true;
73  }
74  if (e->key()==Qt::Key_R){
75  e->accept();
76  goHome=true;
77  }
78  if (e->key()==Qt::Key_C){
79  e->accept();
81 
82  }
83  if (e->key()==Qt::Key_Q){
84  e->accept();
85  wantsQuit=true;
86 
87  }
88  if (e->key()==Qt::Key_D){
89  e->accept();
91 
92  }
93 }
94 
95 void QNavigatorWidget::paintEvent ( QPaintEvent * ){
96  QPixmap pixmap(*m_pixmap);
97  QPainter painter(&pixmap);
98  if (trajectorySent)
99  painter.setPen(Qt::red);
100  bool first=true;
101  int oldx=0, oldy=0;
102  //paint the path
103  for (std::list<IntPoint>::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){
104  int x=it->x;
105  int y=height()-it->y;
106  if (! first)
107  painter.drawLine(oldx, oldy, x,y);
108  oldx=x;
109  oldy=y;
110  first=false;
111  }
112  //paint the robot
113  if (drawRobot){
114  painter.setPen(Qt::black);
115  int rx=robotPose.x;
116  int ry=height()-robotPose.y;
117  int robotSize=6;
118  painter.drawLine(rx, ry,
119  rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading)));
120  painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize);
121  }
122  if (writeImages){
123  dumper.dump(pixmap);
124  }
125  bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP);
126 }
QPixmapDumper dumper
point< int > IntPoint
Definition: point.h:201
GMapping::IntPoint robotPose
virtual void paintEvent(QPaintEvent *paintevent)
QPixmap * m_pixmap
Definition: qmappainter.h:26
virtual ~QNavigatorWidget()
virtual void mousePressEvent(QMouseEvent *e)
virtual void keyPressEvent(QKeyEvent *e)
bool dump(const QPixmap &pixmap)
std::list< GMapping::IntPoint > trajectoryPoints
QNavigatorWidget(QWidget *parent=0, const char *name=0, WFlags f=0)


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20