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);
70 // connect(vidle, SIGNAL(valueChanged(int)),
71 // robotEstPosBest, SLOT(setVidle(int)));
73 setFocusPolicy(Qt::StrongFocus);
74 sharedMemoryOpened = false;
75 WDBG("Youuuhouuuu!!");
78 /**********************************************************************
80 **********************************************************************/
81 void RobomonAtlantis::createLeftLayout()
83 leftLayout = new QVBoxLayout();
85 createDebugGroupBox();
86 debugWindowEnabled = true;
87 createPlaygroundGroupBox();
88 leftLayout->addWidget(playgroundGroupBox);
89 //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
92 void RobomonAtlantis::createRightLayout()
94 rightLayout = new QVBoxLayout();
96 createPositionGroupBox();
99 createActuatorsGroupBox();
100 createPowerGroupBox();
102 rightLayout->addWidget(positionGroupBox);
103 rightLayout->addWidget(miscGroupBox);
104 rightLayout->addWidget(fsmGroupBox);
105 rightLayout->addWidget(powerGroupBox);
106 rightLayout->addWidget(actuatorsGroupBox);
109 void RobomonAtlantis::createPlaygroundGroupBox()
111 playgroundGroupBox = new QGroupBox(tr("Playground"));
112 QHBoxLayout *layout = new QHBoxLayout();
114 playgroundScene = new PlaygroundScene();
115 playgroundSceneView = new PlaygroundView(playgroundScene);
116 //playgroundSceneView->setMinimumWidth(630);
117 //playgroundSceneView->setMinimumHeight(445);
118 playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
119 playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
120 playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
121 playgroundSceneView->setMouseTracking(true);
122 layout->addWidget(playgroundSceneView);
124 playgroundGroupBox->setLayout(layout);
127 void RobomonAtlantis::createPositionGroupBox()
129 positionGroupBox = new QGroupBox(tr("Position state"));
130 positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
131 QGridLayout *layout = new QGridLayout();
133 actPosX = new QLineEdit();
134 actPosY = new QLineEdit();
135 actPosPhi = new QLineEdit();
137 estPosX = new QLineEdit();
138 estPosY = new QLineEdit();
139 estPosPhi = new QLineEdit();
141 actPosX->setReadOnly(true);
142 actPosY->setReadOnly(true);
143 actPosPhi->setReadOnly(true);
145 estPosX->setReadOnly(true);
146 estPosY->setReadOnly(true);
147 estPosPhi->setReadOnly(true);
149 layout->addWidget(MiscGui::createLabel("X"), 1, 0);
150 layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
151 layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
153 layout->addWidget(MiscGui::createLabel("X"), 5, 0);
154 layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
155 layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
157 layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
158 layout->addWidget(actPosX, 1, 1);
159 layout->addWidget(actPosY, 2, 1);
160 layout->addWidget(actPosPhi, 3, 1);
162 layout->addWidget(MiscGui::createLabel("Estimated (indep. odo.)", Qt::AlignLeft), 4, 1);
163 layout->addWidget(estPosX, 5, 1);
164 layout->addWidget(estPosY, 6, 1);
165 layout->addWidget(estPosPhi, 7, 1);
167 positionGroupBox->setLayout(layout);
170 void RobomonAtlantis::createMiscGroupBox()
172 miscGroupBox = new QGroupBox(tr("Miscellaneous"));
173 miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
174 QGridLayout *layout = new QGridLayout();
176 obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
177 obstacleSimulationCheckBox->setShortcut(tr("o"));
178 layout->addWidget(obstacleSimulationCheckBox);
180 startPlug = new QCheckBox("&Start plug");
181 layout->addWidget(startPlug);
183 colorChoser = new QCheckBox("&Team color");
184 layout->addWidget(colorChoser);
186 miscGroupBox->setLayout(layout);
189 void RobomonAtlantis::createFSMGroupBox()
191 fsmGroupBox = new QGroupBox(tr("FSM"));
192 fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
193 QGridLayout *layout = new QGridLayout();
195 layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
196 fsm_main_state = new QLabel();
197 fsm_main_state->setMinimumWidth(100);
198 fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
199 layout->addWidget(fsm_main_state, 1, 1);
201 layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
202 fsm_act_state = new QLabel();
203 layout->addWidget(fsm_act_state, 2, 1);
205 layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
206 fsm_motion_state = new QLabel();
207 layout->addWidget(fsm_motion_state, 3, 1);
209 fsmGroupBox->setLayout(layout);
212 void RobomonAtlantis::createDebugGroupBox()
214 debugGroupBox = new QGroupBox(tr("Debug window"));
215 debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
216 QHBoxLayout *layout = new QHBoxLayout();
218 debugWindow = new QTextEdit();
219 debugWindow->setReadOnly(true);
221 layout->addWidget(debugWindow);
222 debugGroupBox->setLayout(layout);
225 void RobomonAtlantis::createActuatorsGroupBox()
227 actuatorsGroupBox = new QGroupBox(tr("Actuators"));
228 actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
229 QHBoxLayout *layout = new QHBoxLayout();
230 // vidle = new QDial();
232 // vidle->setMinimum(VIDLE_VYSIP);
233 // vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP);
234 // vidle->setEnabled(true);
236 //createMotorsGroupBox();
238 layout->setAlignment(Qt::AlignLeft);
239 // layout->addWidget(vidle);
240 //layout->addWidget(enginesGroupBox);
241 actuatorsGroupBox->setLayout(layout);
244 void RobomonAtlantis::createPowerGroupBox()
246 powerGroupBox = new QGroupBox(tr("Power management"));
247 powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
248 QGridLayout *layout = new QGridLayout();
250 voltage33CheckBox = new QCheckBox(tr("&3.3V"));
251 voltage50CheckBox = new QCheckBox(tr("&5.0V"));
252 voltage80CheckBox = new QCheckBox(tr("&8.0V"));
254 voltage33CheckBox->setShortcut(tr("3"));
255 voltage50CheckBox->setShortcut(tr("5"));
256 voltage80CheckBox->setShortcut(tr("8"));
258 layout->addWidget(voltage33CheckBox, 0, 0);
259 layout->addWidget(voltage50CheckBox, 1, 0);
260 layout->addWidget(voltage80CheckBox, 2, 0);
261 layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
263 voltage33LineEdit = new QLineEdit();
264 voltage50LineEdit = new QLineEdit();
265 voltage80LineEdit = new QLineEdit();
266 voltageBATLineEdit = new QLineEdit();
268 voltage33LineEdit->setReadOnly(true);
269 voltage50LineEdit->setReadOnly(true);
270 voltage80LineEdit->setReadOnly(true);
271 voltageBATLineEdit->setReadOnly(true);
273 layout->addWidget(voltage33LineEdit, 0, 1);
274 layout->addWidget(voltage50LineEdit, 1, 1);
275 layout->addWidget(voltage80LineEdit, 2, 1);
276 layout->addWidget(voltageBATLineEdit, 3, 1);
278 powerGroupBox->setLayout(layout);
281 void RobomonAtlantis::createRobots()
283 robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
284 robotRefPos->setZValue(11);
285 trailRefPos = new Trail(QPen(Qt::darkBlue));
286 trailRefPos->setZValue(11);
288 robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray));
289 robotEstPosBest->setZValue(10);
290 trailEstPosBest = new Trail(QPen());
291 trailEstPosBest->setZValue(10);
293 robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
294 robotEstPosOdo->setZValue(10);
295 trailOdoPos = new Trail(QPen(Qt::red));
296 trailOdoPos->setZValue(10);
298 robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen));
299 robotEstPosIndepOdo->setZValue(10);
300 trailPosIndepOdo = new Trail(QPen(Qt::green));
301 trailPosIndepOdo->setZValue(10);
303 playgroundScene->addItem(robotRefPos);
304 playgroundScene->addItem(robotEstPosBest);
305 playgroundScene->addItem(robotEstPosIndepOdo);
306 playgroundScene->addItem(robotEstPosOdo);
310 playgroundScene->addItem(trailRefPos);
311 playgroundScene->addItem(trailPosIndepOdo);
312 playgroundScene->addItem(trailOdoPos);
314 hokuyoScan = new HokuyoScan();
315 hokuyoScan->setZValue(10);
316 playgroundScene->addItem(hokuyoScan);
320 void RobomonAtlantis::createMap()
322 mapImage = new Map();
323 mapImage->setZValue(5);
324 mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
327 playgroundScene->addItem(mapImage);
330 /**********************************************************************
332 **********************************************************************/
333 void RobomonAtlantis::createActions()
335 /* power management */
336 connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
337 this, SLOT(setVoltage33(int)));
338 connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
339 this, SLOT(setVoltage50(int)));
340 connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
341 this, SLOT(setVoltage80(int)));
344 // connect(leftMotorSlider, SIGNAL(valueChanged(int)),
345 // this, SLOT(setLeftMotor(int)));
346 // connect(rightMotorSlider, SIGNAL(valueChanged(int)),
347 // this, SLOT(setRightMotor(int)));
348 // connect(stopMotorsPushButton, SIGNAL(clicked()),
349 // this, SLOT(stopMotors()));
351 connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
352 connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
354 /* obstacle simulation */
355 simulationEnabled = 0;
356 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
357 this, SLOT(setSimulation(int)));
358 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
359 this, SLOT(setObstacleSimulation(int)));
360 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
361 playgroundScene, SLOT(showObstacle(int)));
362 connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
363 this, SLOT(changeObstacle(QPointF)));
366 void RobomonAtlantis::setVoltage33(int state)
369 orte.pwr_ctrl.voltage33 = true;
371 orte.pwr_ctrl.voltage33 = false;
374 void RobomonAtlantis::setVoltage50(int state)
377 orte.pwr_ctrl.voltage50 = true;
379 orte.pwr_ctrl.voltage50 = false;
382 void RobomonAtlantis::setVoltage80(int state)
385 orte.pwr_ctrl.voltage80 = true;
387 orte.pwr_ctrl.voltage80 = false;
390 // void RobomonAtlantis::setLeftMotor(int value)
392 // short int leftMotor;
393 // short int rightMotor;
395 // if(bothMotorsCheckBox->isChecked())
396 // rightMotorSlider->setValue(value);
398 // leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
399 // rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
401 // orte.motion_speed.left = leftMotor;
402 // orte.motion_speed.right = rightMotor;
406 // void RobomonAtlantis::setRightMotor(int value)
408 // short int leftMotor;
409 // short int rightMotor;
411 // if(bothMotorsCheckBox->isChecked())
412 // leftMotorSlider->setValue(value);
414 // leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
415 // rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
417 // orte.motion_speed.left = leftMotor;
418 // orte.motion_speed.right = rightMotor;
422 // void RobomonAtlantis::stopMotors()
424 // leftMotorSlider->setValue(0);
425 // rightMotorSlider->setValue(0);
428 void RobomonAtlantis::useOpenGL(bool use)
430 playgroundSceneView->useOpenGL(&use);
433 void RobomonAtlantis::showMap(bool show)
437 if (sharedMemoryOpened == false)
441 mapTimer = new QTimer(this);
442 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
443 mapTimer->start(200);
445 if(mapTimer != NULL) {
447 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
450 mapImage->setVisible(show);
453 void RobomonAtlantis::paintMap()
456 struct map *map = ShmapIsMapInit();
460 for(int i = 0; i < MAP_WIDTH; i++) {
461 for(int j = 0; j < MAP_HEIGHT; j++) {
464 struct map_cell *cell = &map->cells[j][i];
467 if ((cell->flags & MAP_FLAG_WALL) &&
468 (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
470 if (cell->flags & MAP_FLAG_IGNORE_OBST)
472 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
474 if (cell->flags & MAP_FLAG_PATH)
476 if (cell->flags & MAP_FLAG_START)
478 if (cell->flags & MAP_FLAG_GOAL)
480 if (cell->flags & MAP_FLAG_PLAN_MARGIN) {
481 QColor c(240, 170, 50); /* orange */
484 if (cell->detected_obstacle) {
485 QColor c1(color), c2(blue);
486 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
487 QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
488 c1.green() + (int)(f*(c2.green() - c1.green())),
489 c1.blue() + (int)(f*(c2.blue() - c1.blue())));
492 if (cell->flags & MAP_FLAG_DET_OBST)
496 mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
501 void RobomonAtlantis::setSimulation(int state)
504 robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
506 if (!simulationEnabled)
508 robottype_publisher_hokuyo_scan_destroy(&orte);
510 simulationEnabled = state;
514 \fn RobomonAtlantis::setObstacleSimulation(int state)
516 void RobomonAtlantis::setObstacleSimulation(int state)
519 /* TODO Maybe it is possible to attach only once to Shmap */
521 obstacleSimulationTimer = new QTimer(this);
522 connect(obstacleSimulationTimer, SIGNAL(timeout()),
523 this, SLOT(simulateObstaclesHokuyo()));
524 obstacleSimulationTimer->start(100);
525 setMouseTracking(true);
527 if (obstacleSimulationTimer)
528 delete obstacleSimulationTimer;
529 //double distance = 0.8;
534 void RobomonAtlantis::simulateObstaclesHokuyo()
536 double distance, wall_distance;
538 uint16_t *hokuyo = orte.hokuyo_scan.data;
540 for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
541 wall_distance = distanceToWallHokuyo(i);
543 distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
544 if (wall_distance < distance)
545 distance = wall_distance;
546 hokuyo[i] = distance*1000;
548 ORTEPublicationSend(orte.publication_hokuyo_scan);
552 void RobomonAtlantis::changeObstacle(QPointF position)
554 if (!simulationEnabled) {
555 simulationEnabled = 1;
556 obstacleSimulationCheckBox->setChecked(true);
559 simulatedObstacle.x = position.x();
560 simulatedObstacle.y = position.y();
561 simulateObstaclesHokuyo();
564 /**********************************************************************
566 **********************************************************************/
567 bool RobomonAtlantis::event(QEvent *event)
569 switch (event->type()) {
570 case QEVENT(QEV_MOTION_STATUS):
571 emit motionStatusReceivedSignal();
573 case QEVENT(QEV_HOKUYO_SCAN):
574 hokuyoScan->newScan(&orte.hokuyo_scan);
576 case QEVENT(QEV_VIDLE_CMD):
577 robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
579 case QEVENT(QEV_REFERENCE_POSITION):
580 emit actualPositionReceivedSignal();
582 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
583 estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
584 estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
585 estPosPhi->setText(QString("%1(%2)")
586 .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
587 .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
588 robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
589 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
590 trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
591 orte.est_pos_indep_odo.y));
593 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
594 robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
595 orte.est_pos_odo.y, orte.est_pos_odo.phi);
596 trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
597 orte.est_pos_odo.y));
599 case QEVENT(QEV_ESTIMATED_POSITION_BEST):
600 robotEstPosBest->moveRobot(orte.est_pos_best.x,
601 orte.est_pos_best.y, orte.est_pos_best.phi);
602 trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x,
603 orte.est_pos_best.y));
604 hokuyoScan->setPosition(orte.est_pos_best.x,
606 orte.est_pos_best.phi);
608 case QEVENT(QEV_POWER_VOLTAGE):
609 emit powerVoltageReceivedSignal();
611 case QEVENT(QEV_FSM_MAIN):
612 fsm_main_state->setText(orte.fsm_main.state_name);
614 case QEVENT(QEV_FSM_ACT):
615 fsm_act_state->setText(orte.fsm_act.state_name);
617 case QEVENT(QEV_FSM_MOTION):
618 fsm_motion_state->setText(orte.fsm_motion.state_name);
621 if (event->type() == QEvent::Close)
622 closeEvent((QCloseEvent *)event);
623 else if (event->type() == QEvent::KeyPress)
624 keyPressEvent((QKeyEvent *)event);
625 else if (event->type() == QEvent::KeyRelease)
626 keyReleaseEvent((QKeyEvent *)event);
627 else if (event->type() == QEvent::FocusIn)
629 else if (event->type() == QEvent::FocusOut)
641 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
643 // double peak, gain;
645 if (event->isAutoRepeat()) {
646 switch (event->key()) {
647 // case Qt::Key_Down:
648 // peak = leftMotorSlider->minimum()/2;
649 // if (leftMotorValue < peak ||
650 // rightMotorValue < peak)
654 // leftMotorValue *= gain;
655 // rightMotorValue *= gain;
656 // leftMotorSlider->setValue((int)leftMotorValue);
657 // rightMotorSlider->setValue((int)rightMotorValue);
661 // case Qt::Key_Left:
662 // case Qt::Key_Right:
663 // peak = leftMotorSlider->maximum()/2;
664 // if (leftMotorValue > peak ||
665 // rightMotorValue > peak)
669 // leftMotorValue *= gain;
670 // rightMotorValue *= gain;
671 // leftMotorSlider->setValue((int)leftMotorValue);
672 // rightMotorSlider->setValue((int)rightMotorValue);
682 switch (event->key()) {
684 // leftMotorValue = 1;
685 // rightMotorValue = 1;
686 // bothMotorsCheckBox->setChecked(true);
687 // leftMotorSlider->setValue((int)leftMotorValue);
688 // setLeftMotor((int)leftMotorValue);
690 // case Qt::Key_Down:
691 // leftMotorValue = -1;
692 // rightMotorValue = -1;
693 // bothMotorsCheckBox->setChecked(true);
694 // leftMotorSlider->setValue((int)leftMotorValue);
695 // setLeftMotor((int)leftMotorValue);
697 // case Qt::Key_Left:
698 // leftMotorValue = -1;
699 // rightMotorValue = 1;
700 // leftMotorSlider->setValue((int)leftMotorValue);
701 // rightMotorSlider->setValue((int)rightMotorValue);
702 // setLeftMotor((int)leftMotorValue);
703 // setRightMotor((int)leftMotorValue);
705 // case Qt::Key_Right:
706 // leftMotorValue = 1;
707 // rightMotorValue = -1;
708 // leftMotorSlider->setValue((int)leftMotorValue);
709 // rightMotorSlider->setValue((int)rightMotorValue);
710 // setLeftMotor((int)leftMotorValue);
711 // setRightMotor((int)rightMotorValue);
720 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
722 if (event->isAutoRepeat()) {
727 switch (event->key()) {
729 // case Qt::Key_Down:
730 // case Qt::Key_Left:
731 // case Qt::Key_Right:
732 // leftMotorValue = 0;
733 // rightMotorValue = 0;
734 // bothMotorsCheckBox->setChecked(false);
735 // leftMotorSlider->setValue((int)leftMotorValue);
736 // rightMotorSlider->setValue((int)rightMotorValue);
745 void RobomonAtlantis::closeEvent(QCloseEvent *)
747 robottype_roboorte_destroy(&orte);
750 /**********************************************************************
752 **********************************************************************/
753 void RobomonAtlantis::createOrte()
759 memset(&orte, 0, sizeof(orte));
760 rv = robottype_roboorte_init(&orte);
762 printf("RobomonAtlantis: Unable to initialize ORTE\n");
766 robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
768 robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
769 robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
770 robottype_publisher_robot_switches_create(&orte, dummy_publisher_callback, &orte);
773 robottype_subscriber_pwr_voltage_create(&orte,
774 receivePowerVoltageCallBack, this);
775 robottype_subscriber_motion_status_create(&orte,
776 receiveMotionStatusCallBack, this);
777 robottype_subscriber_ref_pos_create(&orte,
778 receiveActualPositionCallBack, this);
779 robottype_subscriber_est_pos_odo_create(&orte,
780 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
781 robottype_subscriber_est_pos_indep_odo_create(&orte,
782 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
783 robottype_subscriber_est_pos_best_create(&orte,
784 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
785 robottype_subscriber_hokuyo_scan_create(&orte,
786 generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
787 robottype_subscriber_vidle_cmd_create(&orte,
788 generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD));
789 robottype_subscriber_fsm_main_create(&orte,
790 rcv_fsm_main_cb, this);
791 robottype_subscriber_fsm_motion_create(&orte,
792 rcv_fsm_motion_cb, this);
793 robottype_subscriber_fsm_act_create(&orte,
794 rcv_fsm_act_cb, this);
797 orte.motion_speed.left = 0;
798 orte.motion_speed.right = 0;
800 /* power management */
801 orte.pwr_ctrl.voltage33 = true;
802 orte.pwr_ctrl.voltage50 = true;
803 orte.pwr_ctrl.voltage80 = true;
804 voltage33CheckBox->setChecked(true);
805 voltage50CheckBox->setChecked(true);
806 voltage80CheckBox->setChecked(true);
810 /* set actions to do when we receive data from orte */
811 connect(this, SIGNAL(motionStatusReceivedSignal()),
812 this, SLOT(motionStatusReceived()));
813 connect(this, SIGNAL(actualPositionReceivedSignal()),
814 this, SLOT(actualPositionReceived()));
815 connect(this, SIGNAL(powerVoltageReceivedSignal()),
816 this, SLOT(powerVoltageReceived()));
819 void RobomonAtlantis::motionStatusReceived()
821 WDBG("ORTE received: motion status");
824 void RobomonAtlantis::actualPositionReceived()
826 actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
827 actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
828 actPosPhi->setText(QString("%1(%2)")
829 .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
830 .arg(orte.ref_pos.phi, 0, 'f', 1));
831 robotRefPos->moveRobot(orte.ref_pos.x,
832 orte.ref_pos.y, orte.ref_pos.phi);
833 trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
836 void RobomonAtlantis::powerVoltageReceived()
838 voltage33LineEdit->setText(QString("%1").arg(
839 orte.pwr_voltage.voltage33, 0, 'f', 3));
840 voltage50LineEdit->setText(QString("%1").arg(
841 orte.pwr_voltage.voltage50, 0, 'f', 3));
842 voltage80LineEdit->setText(QString("%1").arg(
843 orte.pwr_voltage.voltage80, 0, 'f', 3));
844 voltageBATLineEdit->setText(QString("%1").arg(
845 orte.pwr_voltage.voltageBAT, 0, 'f', 3));
849 /**********************************************************************
851 **********************************************************************/
852 void RobomonAtlantis::openSharedMemory()
855 int sharedSegmentSize;
857 if (sharedMemoryOpened)
860 sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
862 /* Get segment identificator in a read only mode */
863 segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
864 if(segmentId == -1) {
865 QMessageBox::critical(this, "robomon",
866 "Unable to open shared memory segment!");
873 /* Attach the shared memory segment */
874 //map = (_Map*)shmat (segmentId, (void*) 0, 0);
876 sharedMemoryOpened = true;
879 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
881 double distance=4.0, min_distance=4.0;
884 struct map *map = ShmapIsMapInit();
886 if (!map) return min_distance;
888 // Simulate obstacles
889 for(j=0;j<MAP_HEIGHT;j++) {
890 for (i=0;i<MAP_WIDTH;i++) {
891 struct map_cell *cell = &map->cells[j][i];
892 if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
894 ShmapCell2Point(i, j, &wall.x, &wall.y);
896 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
897 if (distance<min_distance) min_distance = distance;
905 double RobomonAtlantis::distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter)
907 struct robot_pos_type e = orte.est_pos_best;
911 s.x = HOKUYO_CENTER_OFFSET_M;
913 s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
915 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
916 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
917 sensor_a = e.phi + s.ang;
919 const double sensorRange = 4.0; /*[meters]*/
921 double distance = sensorRange;
924 angle = sensor.angleTo(center) - sensor_a;
925 angle = fmod(angle, 2.0*M_PI);
926 if (angle > +M_PI) angle -= 2.0*M_PI;
927 if (angle < -M_PI) angle += 2.0*M_PI;
930 double k = tan(sensor_a);
931 double r = diameter / 2.0;
934 double B = 2 * (sensor.y*k - center.x - k*k*sensor.x - center.y*k);
935 double C = center.x*center.x + center.y*center.y +
936 k*k*sensor.x*sensor.x - 2*sensor.y*k*sensor.x +
937 sensor.y*sensor.y + 2*k*sensor.x*center.y -
938 2*sensor.y*center.y - r*r;
940 double D = B*B - 4*A*C;
945 ob1.x = (-B + sqrt(D)) / (2*A);
946 ob2.x = (-B - sqrt(D)) / (2*A);
947 ob1.y = k * (ob1.x - sensor.x) + sensor.y;
948 ob2.y = k * (ob2.x - sensor.x) + sensor.y;
950 double distance1 = sensor.distanceTo(ob1);
951 double distance2 = sensor.distanceTo(ob2);
952 distance = (distance1 < distance2) ? distance1 : distance2;
956 ob.y = k * (ob.x - sensor.x) + sensor.y;
957 distance = sensor.distanceTo(ob);
959 if (D < 0 || angle > atan(r / distance))
960 distance = sensorRange;
961 if (distance > sensorRange)
962 distance = sensorRange;
968 * Calculation for Hokuyo simulation. Calculates distance that would
969 * be returned by Hokuyo sensors, if there is only one obstacle (as
970 * specified by parameters).
972 * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
973 * @param obstacle Position of the obstacle (x, y in meters).
974 * @param obstacleSize Size (diameter) of the obstacle in meters.
976 * @return Distance measured by sensors in meters.
978 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
980 struct robot_pos_type e = orte.est_pos_best;
984 s.x = HOKUYO_CENTER_OFFSET_M;
986 s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
988 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
989 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
990 sensor_a = e.phi + s.ang;
992 const double sensorRange = 4.0; /*[meters]*/
994 double distance, angle;
996 angle = sensor.angleTo(obstacle) - sensor_a;
997 angle = fmod(angle, 2.0*M_PI);
998 if (angle > +M_PI) angle -= 2.0*M_PI;
999 if (angle < -M_PI) angle += 2.0*M_PI;
1000 angle = fabs(angle);
1001 distance = sensor.distanceTo(obstacle) - obstacleSize/2.0;
1002 if (angle < atan(obstacleSize/2.0 / distance)) {
1003 // We can see the obstackle from here.
1004 if (angle < M_PI/2.0) {
1005 distance = distance/cos(angle);
1007 if (distance > sensorRange)
1008 distance = sensorRange;
1010 distance = sensorRange;
1016 void RobomonAtlantis::sendStart(int plug)
1018 orte.robot_cmd.start_condition = plug ? 0 : 1;
1019 ORTEPublicationSend(orte.publication_robot_cmd);
1022 void RobomonAtlantis::setTeamColor(int plug)
1024 orte.robot_switches.team_color = plug ? 1 : 0;
1025 ORTEPublicationSend(orte.publication_robot_switches);
1028 void RobomonAtlantis::resetTrails()
1030 trailRefPos->reset();
1031 trailEstPosBest->reset();
1032 trailPosIndepOdo->reset();
1033 trailOdoPos->reset();
1036 void RobomonAtlantis::showTrails(bool show)
1038 trailRefPos->setVisible(show && robotRefPos->isVisible());
1039 trailEstPosBest->setVisible(show && robotEstPosBest->isVisible());
1040 trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible());
1041 trailOdoPos->setVisible(show && robotEstPosOdo->isVisible());
1044 void RobomonAtlantis::showShapeDetect(bool show)
1046 hokuyoScan->showShapeDetect = show;