2 * RobomonAtlantis.cpp 07/10/31
4 * Robot`s visualization and control GUI for robot of the
5 * Eurobot 2008 (Mission to Mars).
7 * Copyright: (c) 2008 CTU Dragons
8 * CTU FEE - Department of Control Engineering
9 * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
10 * License: GNU GPL v.2
18 #include <sys/socket.h>
19 #include <netinet/in.h>
20 #include <arpa/inet.h>
27 #include <path_planner.h>
34 #include <actuators.h>
35 #include "PlaygroundScene.h"
37 #include "robomon_orte.h"
38 #include "RobomonAtlantis.h"
39 #include "playgroundview.h"
42 #include <QCoreApplication>
46 #include <QMessageBox>
48 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
55 debugWindowEnabled = false;
60 QHBoxLayout *mainLayout = new QHBoxLayout;
61 mainLayout->addLayout(leftLayout);
62 mainLayout->addLayout(rightLayout);
63 setLayout(mainLayout);
69 setFocusPolicy(Qt::StrongFocus);
70 sharedMemoryOpened = false;
71 WDBG("Youuuhouuuu!!");
74 /**********************************************************************
76 **********************************************************************/
77 void RobomonAtlantis::createLeftLayout()
79 leftLayout = new QVBoxLayout();
81 createDebugGroupBox();
82 debugWindowEnabled = true;
83 createPlaygroundGroupBox();
84 leftLayout->addWidget(playgroundGroupBox);
85 //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
88 void RobomonAtlantis::createRightLayout()
90 rightLayout = new QVBoxLayout();
92 createPositionGroupBox();
95 createActuatorsGroupBox();
96 createPowerGroupBox();
98 rightLayout->addWidget(positionGroupBox);
99 rightLayout->addWidget(miscGroupBox);
100 rightLayout->addWidget(fsmGroupBox);
101 rightLayout->addWidget(powerGroupBox);
102 rightLayout->addWidget(actuatorsGroupBox);
105 void RobomonAtlantis::createPlaygroundGroupBox()
107 playgroundGroupBox = new QGroupBox(tr("Playground"));
108 QHBoxLayout *layout = new QHBoxLayout();
110 playgroundScene = new PlaygroundScene();
111 playgroundSceneView = new PlaygroundView(playgroundScene);
112 //playgroundSceneView->setMinimumWidth(630);
113 //playgroundSceneView->setMinimumHeight(445);
114 playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
115 playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
116 playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
117 playgroundSceneView->setMouseTracking(true);
118 layout->addWidget(playgroundSceneView);
120 playgroundGroupBox->setLayout(layout);
123 void RobomonAtlantis::createPositionGroupBox()
125 positionGroupBox = new QGroupBox(tr("Position state"));
126 positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
127 QGridLayout *layout = new QGridLayout();
129 actPosX = new QLineEdit();
130 actPosY = new QLineEdit();
131 actPosPhi = new QLineEdit();
133 estPosX = new QLineEdit();
134 estPosY = new QLineEdit();
135 estPosPhi = new QLineEdit();
137 actPosX->setReadOnly(true);
138 actPosY->setReadOnly(true);
139 actPosPhi->setReadOnly(true);
141 estPosX->setReadOnly(true);
142 estPosY->setReadOnly(true);
143 estPosPhi->setReadOnly(true);
145 layout->addWidget(MiscGui::createLabel("X"), 1, 0);
146 layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
147 layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
149 layout->addWidget(MiscGui::createLabel("X"), 5, 0);
150 layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
151 layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
153 layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
154 layout->addWidget(actPosX, 1, 1);
155 layout->addWidget(actPosY, 2, 1);
156 layout->addWidget(actPosPhi, 3, 1);
158 layout->addWidget(MiscGui::createLabel("Estimated (indep. odo.)", Qt::AlignLeft), 4, 1);
159 layout->addWidget(estPosX, 5, 1);
160 layout->addWidget(estPosY, 6, 1);
161 layout->addWidget(estPosPhi, 7, 1);
163 positionGroupBox->setLayout(layout);
166 void RobomonAtlantis::createMiscGroupBox()
168 miscGroupBox = new QGroupBox(tr("Miscellaneous"));
169 miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
170 QGridLayout *layout = new QGridLayout();
172 obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
173 obstacleSimulationCheckBox->setShortcut(tr("o"));
174 layout->addWidget(obstacleSimulationCheckBox);
176 startPlug = new QCheckBox("Start plug");
177 layout->addWidget(startPlug);
179 puckInside = new QCheckBox("Puck inside");
180 layout->addWidget(puckInside);
182 miscGroupBox->setLayout(layout);
185 void RobomonAtlantis::createFSMGroupBox()
187 fsmGroupBox = new QGroupBox(tr("FSM"));
188 fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
189 QGridLayout *layout = new QGridLayout();
191 layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
192 fsm_main_state = new QLabel();
193 fsm_main_state->setMinimumWidth(100);
194 fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
195 layout->addWidget(fsm_main_state, 1, 1);
197 layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
198 fsm_act_state = new QLabel();
199 layout->addWidget(fsm_act_state, 2, 1);
201 layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
202 fsm_motion_state = new QLabel();
203 layout->addWidget(fsm_motion_state, 3, 1);
205 fsmGroupBox->setLayout(layout);
208 void RobomonAtlantis::createDebugGroupBox()
210 debugGroupBox = new QGroupBox(tr("Debug window"));
211 debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
212 QHBoxLayout *layout = new QHBoxLayout();
214 debugWindow = new QTextEdit();
215 debugWindow->setReadOnly(true);
217 layout->addWidget(debugWindow);
218 debugGroupBox->setLayout(layout);
221 void RobomonAtlantis::createActuatorsGroupBox()
223 actuatorsGroupBox = new QGroupBox(tr("Actuators"));
224 actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
225 QHBoxLayout *layout = new QHBoxLayout();
227 //createMotorsGroupBox();
229 layout->setAlignment(Qt::AlignLeft);
230 //layout->addWidget(enginesGroupBox);
231 actuatorsGroupBox->setLayout(layout);
234 void RobomonAtlantis::createPowerGroupBox()
236 powerGroupBox = new QGroupBox(tr("Power management"));
237 powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
238 QGridLayout *layout = new QGridLayout();
240 voltage33CheckBox = new QCheckBox(tr("&3.3V"));
241 voltage50CheckBox = new QCheckBox(tr("&5.0V"));
242 voltage80CheckBox = new QCheckBox(tr("&8.0V"));
244 voltage33CheckBox->setShortcut(tr("3"));
245 voltage50CheckBox->setShortcut(tr("5"));
246 voltage80CheckBox->setShortcut(tr("8"));
248 layout->addWidget(voltage33CheckBox, 0, 0);
249 layout->addWidget(voltage50CheckBox, 1, 0);
250 layout->addWidget(voltage80CheckBox, 2, 0);
251 layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
253 voltage33LineEdit = new QLineEdit();
254 voltage50LineEdit = new QLineEdit();
255 voltage80LineEdit = new QLineEdit();
256 voltageBATLineEdit = new QLineEdit();
258 voltage33LineEdit->setReadOnly(true);
259 voltage50LineEdit->setReadOnly(true);
260 voltage80LineEdit->setReadOnly(true);
261 voltageBATLineEdit->setReadOnly(true);
263 layout->addWidget(voltage33LineEdit, 0, 1);
264 layout->addWidget(voltage50LineEdit, 1, 1);
265 layout->addWidget(voltage80LineEdit, 2, 1);
266 layout->addWidget(voltageBATLineEdit, 3, 1);
268 powerGroupBox->setLayout(layout);
272 void RobomonAtlantis::createMotorsGroupBox()
274 enginesGroupBox = new QGroupBox(tr("Motors"));
275 QVBoxLayout *layout = new QVBoxLayout();
276 QHBoxLayout *layout1 = new QHBoxLayout();
277 QHBoxLayout *layout2 = new QHBoxLayout();
279 leftMotorSlider = new QSlider(Qt::Vertical);
280 rightMotorSlider = new QSlider(Qt::Vertical);
281 bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
282 stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
284 leftMotorSlider->setMinimum(-100);
285 leftMotorSlider->setMaximum(100);
286 leftMotorSlider->setTracking(false);
287 leftMotorSlider->setTickPosition(QSlider::TicksLeft);
289 rightMotorSlider->setMinimum(-100);
290 rightMotorSlider->setMaximum(100);
291 rightMotorSlider->setTracking(false);
292 rightMotorSlider->setTickPosition(QSlider::TicksRight);
294 stopMotorsPushButton->setMaximumWidth(90);
296 layout1->addWidget(leftMotorSlider);
297 layout1->addWidget(MiscGui::createLabel("0"));
298 layout1->addWidget(rightMotorSlider);
300 layout2->addWidget(bothMotorsCheckBox);
302 layout->addWidget(MiscGui::createLabel("100"));
303 layout->addLayout(layout1);
304 layout->addWidget(MiscGui::createLabel("-100"));
305 layout->addLayout(layout2);
306 layout->addWidget(stopMotorsPushButton);
307 enginesGroupBox->setLayout(layout);
311 void RobomonAtlantis::createRobots()
313 robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
314 robotRefPos->setZValue(11);
315 trailRefPos = new Trail(QPen(Qt::darkBlue));
316 trailRefPos->setZValue(11);
318 robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray));
319 robotEstPosBest->setZValue(10);
320 trailEstPosBest = new Trail(QPen());
321 trailEstPosBest->setZValue(10);
323 robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
324 robotEstPosOdo->setZValue(10);
325 trailOdoPos = new Trail(QPen(Qt::red));
326 trailOdoPos->setZValue(10);
328 robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen));
329 robotEstPosIndepOdo->setZValue(10);
330 trailPosIndepOdo = new Trail(QPen(Qt::green));
331 trailPosIndepOdo->setZValue(10);
333 playgroundScene->addItem(robotRefPos);
334 playgroundScene->addItem(robotEstPosBest);
335 playgroundScene->addItem(robotEstPosIndepOdo);
336 playgroundScene->addItem(robotEstPosOdo);
340 playgroundScene->addItem(trailRefPos);
341 playgroundScene->addItem(trailPosIndepOdo);
342 playgroundScene->addItem(trailOdoPos);
344 hokuyoScan = new HokuyoScan();
345 hokuyoScan->setZValue(10);
346 playgroundScene->addItem(hokuyoScan);
350 /**********************************************************************
352 **********************************************************************/
353 void RobomonAtlantis::createActions()
355 /* power management */
356 connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
357 this, SLOT(setVoltage33(int)));
358 connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
359 this, SLOT(setVoltage50(int)));
360 connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
361 this, SLOT(setVoltage80(int)));
364 // connect(leftMotorSlider, SIGNAL(valueChanged(int)),
365 // this, SLOT(setLeftMotor(int)));
366 // connect(rightMotorSlider, SIGNAL(valueChanged(int)),
367 // this, SLOT(setRightMotor(int)));
368 // connect(stopMotorsPushButton, SIGNAL(clicked()),
369 // this, SLOT(stopMotors()));
371 connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
373 /* obstacle simulation */
374 simulationEnabled = 0;
375 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
376 this, SLOT(setSimulation(int)));
377 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
378 this, SLOT(setObstacleSimulation(int)));
379 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
380 playgroundScene, SLOT(showObstacle(int)));
381 connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
382 this, SLOT(changeObstacle(QPointF)));
385 void RobomonAtlantis::setVoltage33(int state)
388 orte.pwr_ctrl.voltage33 = true;
390 orte.pwr_ctrl.voltage33 = false;
393 void RobomonAtlantis::setVoltage50(int state)
396 orte.pwr_ctrl.voltage50 = true;
398 orte.pwr_ctrl.voltage50 = false;
401 void RobomonAtlantis::setVoltage80(int state)
404 orte.pwr_ctrl.voltage80 = true;
406 orte.pwr_ctrl.voltage80 = false;
409 // void RobomonAtlantis::setLeftMotor(int value)
411 // short int leftMotor;
412 // short int rightMotor;
414 // if(bothMotorsCheckBox->isChecked())
415 // rightMotorSlider->setValue(value);
417 // leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
418 // rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
420 // orte.motion_speed.left = leftMotor;
421 // orte.motion_speed.right = rightMotor;
425 // void RobomonAtlantis::setRightMotor(int value)
427 // short int leftMotor;
428 // short int rightMotor;
430 // if(bothMotorsCheckBox->isChecked())
431 // leftMotorSlider->setValue(value);
433 // leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
434 // rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
436 // orte.motion_speed.left = leftMotor;
437 // orte.motion_speed.right = rightMotor;
441 // void RobomonAtlantis::stopMotors()
443 // leftMotorSlider->setValue(0);
444 // rightMotorSlider->setValue(0);
447 void RobomonAtlantis::useOpenGL(bool use)
449 playgroundSceneView->useOpenGL(&use);
452 void RobomonAtlantis::showMap(bool show)
456 if (sharedMemoryOpened == false)
460 mapTimer = new QTimer(this);
461 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
462 mapTimer->start(200);
464 if(mapTimer != NULL) {
466 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
469 playgroundScene->showMap(show);
472 void RobomonAtlantis::paintMap()
475 struct map *map = ShmapIsMapInit();
479 for(int i=0; i < MAP_WIDTH; i++) {
480 for(int j=0; j<MAP_HEIGHT; j++) {
483 struct map_cell *cell = &map->cells[j][i];
486 if ((cell->flags & MAP_FLAG_WALL) &&
487 (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
489 if (cell->flags & MAP_FLAG_IGNORE_OBST)
491 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
493 if (cell->flags & MAP_FLAG_PATH)
495 if (cell->flags & MAP_FLAG_START)
497 if (cell->flags & MAP_FLAG_GOAL)
499 if (cell->flags & MAP_FLAG_PLAN_MARGIN) {
500 QColor c(240, 170, 50); /* orange */
503 if (cell->detected_obstacle) {
504 QColor c1(color), c2(blue);
505 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
506 QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
507 c1.green() + (int)(f*(c2.green() - c1.green())),
508 c1.blue() + (int)(f*(c2.blue() - c1.blue())));
511 if (cell->flags & MAP_FLAG_DET_OBST)
514 playgroundScene->setMapColor(i, j, color);
519 void RobomonAtlantis::setSimulation(int state)
522 robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
524 if (!simulationEnabled)
526 robottype_publisher_hokuyo_scan_destroy(&orte);
528 simulationEnabled = state;
532 \fn RobomonAtlantis::setObstacleSimulation(int state)
534 void RobomonAtlantis::setObstacleSimulation(int state)
537 /* TODO Maybe it is possible to attach only once to Shmap */
539 obstacleSimulationTimer = new QTimer(this);
540 connect(obstacleSimulationTimer, SIGNAL(timeout()),
541 this, SLOT(simulateObstaclesHokuyo()));
542 obstacleSimulationTimer->start(100);
543 setMouseTracking(true);
545 if (obstacleSimulationTimer)
546 delete obstacleSimulationTimer;
547 //double distance = 0.8;
552 void RobomonAtlantis::simulateObstaclesHokuyo()
554 double distance, wall_distance;
556 uint16_t *hokuyo = orte.hokuyo_scan.data;
558 for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
559 wall_distance = distanceToWallHokuyo(i);
560 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
561 if (wall_distance < distance)
562 distance = wall_distance;
563 hokuyo[i] = distance*1000;
565 ORTEPublicationSend(orte.publication_hokuyo_scan);
569 void RobomonAtlantis::changeObstacle(QPointF position)
571 if (!simulationEnabled) {
572 simulationEnabled = 1;
573 obstacleSimulationCheckBox->setChecked(true);
576 simulatedObstacle.x = position.x();
577 simulatedObstacle.y = position.y();
578 simulateObstaclesHokuyo();
581 /**********************************************************************
583 **********************************************************************/
584 bool RobomonAtlantis::event(QEvent *event)
586 switch (event->type()) {
587 case QEVENT(QEV_MOTION_STATUS):
588 emit motionStatusReceivedSignal();
590 case QEVENT(QEV_HOKUYO_SCAN):
591 hokuyoScan->newScan(&orte.hokuyo_scan);
593 case QEVENT(QEV_REFERENCE_POSITION):
594 emit actualPositionReceivedSignal();
596 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
597 estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
598 estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
599 estPosPhi->setText(QString("%1(%2)")
600 .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
601 .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
602 robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
603 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
604 trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
605 orte.est_pos_indep_odo.y));
607 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
608 robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
609 orte.est_pos_odo.y, orte.est_pos_odo.phi);
610 trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
611 orte.est_pos_odo.y));
613 case QEVENT(QEV_ESTIMATED_POSITION_BEST):
614 robotEstPosBest->moveRobot(orte.est_pos_best.x,
615 orte.est_pos_best.y, orte.est_pos_best.phi);
616 trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x,
617 orte.est_pos_best.y));
618 hokuyoScan->setPosition(orte.est_pos_best.x,
620 orte.est_pos_best.phi);
622 case QEVENT(QEV_POWER_VOLTAGE):
623 emit powerVoltageReceivedSignal();
625 case QEVENT(QEV_FSM_MAIN):
626 fsm_main_state->setText(orte.fsm_main.state_name);
628 case QEVENT(QEV_FSM_ACT):
629 fsm_act_state->setText(orte.fsm_act.state_name);
631 case QEVENT(QEV_FSM_MOTION):
632 fsm_motion_state->setText(orte.fsm_motion.state_name);
635 if (event->type() == QEvent::Close)
636 closeEvent((QCloseEvent *)event);
637 else if (event->type() == QEvent::KeyPress)
638 keyPressEvent((QKeyEvent *)event);
639 else if (event->type() == QEvent::KeyRelease)
640 keyReleaseEvent((QKeyEvent *)event);
641 else if (event->type() == QEvent::FocusIn)
643 else if (event->type() == QEvent::FocusOut)
655 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
657 // double peak, gain;
659 if (event->isAutoRepeat()) {
660 switch (event->key()) {
661 // case Qt::Key_Down:
662 // peak = leftMotorSlider->minimum()/2;
663 // if (leftMotorValue < peak ||
664 // rightMotorValue < peak)
668 // leftMotorValue *= gain;
669 // rightMotorValue *= gain;
670 // leftMotorSlider->setValue((int)leftMotorValue);
671 // rightMotorSlider->setValue((int)rightMotorValue);
675 // case Qt::Key_Left:
676 // case Qt::Key_Right:
677 // peak = leftMotorSlider->maximum()/2;
678 // if (leftMotorValue > peak ||
679 // rightMotorValue > peak)
683 // leftMotorValue *= gain;
684 // rightMotorValue *= gain;
685 // leftMotorSlider->setValue((int)leftMotorValue);
686 // rightMotorSlider->setValue((int)rightMotorValue);
696 switch (event->key()) {
698 // leftMotorValue = 1;
699 // rightMotorValue = 1;
700 // bothMotorsCheckBox->setChecked(true);
701 // leftMotorSlider->setValue((int)leftMotorValue);
702 // setLeftMotor((int)leftMotorValue);
704 // case Qt::Key_Down:
705 // leftMotorValue = -1;
706 // rightMotorValue = -1;
707 // bothMotorsCheckBox->setChecked(true);
708 // leftMotorSlider->setValue((int)leftMotorValue);
709 // setLeftMotor((int)leftMotorValue);
711 // case Qt::Key_Left:
712 // leftMotorValue = -1;
713 // rightMotorValue = 1;
714 // leftMotorSlider->setValue((int)leftMotorValue);
715 // rightMotorSlider->setValue((int)rightMotorValue);
716 // setLeftMotor((int)leftMotorValue);
717 // setRightMotor((int)leftMotorValue);
719 // case Qt::Key_Right:
720 // leftMotorValue = 1;
721 // rightMotorValue = -1;
722 // leftMotorSlider->setValue((int)leftMotorValue);
723 // rightMotorSlider->setValue((int)rightMotorValue);
724 // setLeftMotor((int)leftMotorValue);
725 // setRightMotor((int)rightMotorValue);
734 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
736 if (event->isAutoRepeat()) {
741 switch (event->key()) {
743 // case Qt::Key_Down:
744 // case Qt::Key_Left:
745 // case Qt::Key_Right:
746 // leftMotorValue = 0;
747 // rightMotorValue = 0;
748 // bothMotorsCheckBox->setChecked(false);
749 // leftMotorSlider->setValue((int)leftMotorValue);
750 // rightMotorSlider->setValue((int)rightMotorValue);
759 void RobomonAtlantis::closeEvent(QCloseEvent *)
761 robottype_roboorte_destroy(&orte);
764 /**********************************************************************
766 **********************************************************************/
767 void RobomonAtlantis::createOrte()
773 rv = robottype_roboorte_init(&orte);
775 printf("RobomonAtlantis: Unable to initialize ORTE\n");
779 robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
781 robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
782 robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
785 robottype_subscriber_pwr_voltage_create(&orte,
786 receivePowerVoltageCallBack, this);
787 robottype_subscriber_motion_status_create(&orte,
788 receiveMotionStatusCallBack, this);
789 robottype_subscriber_ref_pos_create(&orte,
790 receiveActualPositionCallBack, this);
791 robottype_subscriber_est_pos_odo_create(&orte,
792 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
793 robottype_subscriber_est_pos_indep_odo_create(&orte,
794 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
795 robottype_subscriber_est_pos_best_create(&orte,
796 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
797 robottype_subscriber_hokuyo_scan_create(&orte,
798 generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
799 robottype_subscriber_fsm_main_create(&orte,
800 rcv_fsm_main_cb, this);
801 robottype_subscriber_fsm_motion_create(&orte,
802 rcv_fsm_motion_cb, this);
803 robottype_subscriber_fsm_act_create(&orte,
804 rcv_fsm_act_cb, this);
807 orte.motion_speed.left = 0;
808 orte.motion_speed.right = 0;
810 /* power management */
811 orte.pwr_ctrl.voltage33 = true;
812 orte.pwr_ctrl.voltage50 = true;
813 orte.pwr_ctrl.voltage80 = true;
814 voltage33CheckBox->setChecked(true);
815 voltage50CheckBox->setChecked(true);
816 voltage80CheckBox->setChecked(true);
820 /* set actions to do when we receive data from orte */
821 connect(this, SIGNAL(motionStatusReceivedSignal()),
822 this, SLOT(motionStatusReceived()));
823 connect(this, SIGNAL(actualPositionReceivedSignal()),
824 this, SLOT(actualPositionReceived()));
825 connect(this, SIGNAL(powerVoltageReceivedSignal()),
826 this, SLOT(powerVoltageReceived()));
829 void RobomonAtlantis::motionStatusReceived()
831 WDBG("ORTE received: motion status");
834 void RobomonAtlantis::actualPositionReceived()
836 actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
837 actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
838 actPosPhi->setText(QString("%1(%2)")
839 .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
840 .arg(orte.ref_pos.phi, 0, 'f', 1));
841 robotRefPos->moveRobot(orte.ref_pos.x,
842 orte.ref_pos.y, orte.ref_pos.phi);
843 trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
846 void RobomonAtlantis::powerVoltageReceived()
848 voltage33LineEdit->setText(QString("%1").arg(
849 orte.pwr_voltage.voltage33, 0, 'f', 3));
850 voltage50LineEdit->setText(QString("%1").arg(
851 orte.pwr_voltage.voltage50, 0, 'f', 3));
852 voltage80LineEdit->setText(QString("%1").arg(
853 orte.pwr_voltage.voltage80, 0, 'f', 3));
854 voltageBATLineEdit->setText(QString("%1").arg(
855 orte.pwr_voltage.voltageBAT, 0, 'f', 3));
859 /**********************************************************************
861 **********************************************************************/
862 void RobomonAtlantis::openSharedMemory()
865 int sharedSegmentSize;
867 if (sharedMemoryOpened)
870 sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
872 /* Get segment identificator in a read only mode */
873 segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
874 if(segmentId == -1) {
875 QMessageBox::critical(this, "robomon",
876 "Unable to open shared memory segment!");
883 /* Attach the shared memory segment */
884 //map = (_Map*)shmat (segmentId, (void*) 0, 0);
886 sharedMemoryOpened = true;
889 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
891 double distance=4.0, min_distance=4.0;
894 struct map *map = ShmapIsMapInit();
896 if (!map) return min_distance;
898 // Simulate obstacles
899 for(j=0;j<MAP_HEIGHT;j++) {
900 for (i=0;i<MAP_WIDTH;i++) {
901 struct map_cell *cell = &map->cells[j][i];
902 if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
904 ShmapCell2Point(i, j, &wall.x, &wall.y);
906 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
907 if (distance<min_distance) min_distance = distance;
916 * Calculation for Hokuyo simulation. Calculates distance that would
917 * be returned by Hokuyo sensors, if there is only one obstacle (as
918 * specified by parameters).
920 * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
921 * @param obstacle Position of the obstacle (x, y in meters).
922 * @param obstacleSize Size (diameter) of the obstacle in meters.
924 * @return Distance measured by sensors in meters.
926 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
928 struct robot_pos_type e = orte.est_pos_best;
932 s.x = HOKUYO_CENTER_OFFSET_M;
934 s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
936 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
937 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
938 sensor_a = e.phi + s.ang;
940 const double sensorRange = 4.0; /*[meters]*/
942 double distance, angle;
944 angle = sensor.angleTo(obstacle) - sensor_a;
945 angle = fmod(angle, 2.0*M_PI);
946 if (angle > +M_PI) angle -= 2.0*M_PI;
947 if (angle < -M_PI) angle += 2.0*M_PI;
949 distance = sensor.distanceTo(obstacle) - obstacleSize/2.0;
950 if (angle < atan(obstacleSize/2.0 / distance)) {
951 // We can see the obstackle from here.
952 if (angle < M_PI/2.0) {
953 distance = distance/cos(angle);
955 if (distance > sensorRange)
956 distance = sensorRange;
958 distance = sensorRange;
964 void RobomonAtlantis::sendStart(int plug)
966 orte.robot_cmd.start_condition = plug ? 0 : 1;
967 ORTEPublicationSend(orte.publication_robot_cmd);
970 void RobomonAtlantis::resetTrails()
972 trailRefPos->reset();
973 trailEstPosBest->reset();
974 trailPosIndepOdo->reset();
975 trailOdoPos->reset();
978 void RobomonAtlantis::showTrails(bool show)
980 trailRefPos->setVisible(show && robotRefPos->isVisible());
981 trailEstPosBest->setVisible(show && robotEstPosBest->isVisible());
982 trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible());
983 trailOdoPos->setVisible(show && robotEstPosOdo->isVisible());