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>
49 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
56 debugWindowEnabled = false;
61 QHBoxLayout *mainLayout = new QHBoxLayout;
62 mainLayout->addLayout(leftLayout);
63 mainLayout->addLayout(rightLayout);
64 setLayout(mainLayout);
70 setFocusPolicy(Qt::StrongFocus);
71 sharedMemoryOpened = false;
72 WDBG("Youuuhouuuu!!");
75 /**********************************************************************
77 **********************************************************************/
78 void RobomonAtlantis::createLeftLayout()
80 leftLayout = new QVBoxLayout();
82 createDebugGroupBox();
83 debugWindowEnabled = true;
84 createPlaygroundGroupBox();
85 leftLayout->addWidget(playgroundGroupBox);
86 //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
89 void RobomonAtlantis::createRightLayout()
91 rightLayout = new QVBoxLayout();
93 createPositionGroupBox();
96 createActuatorsGroupBox();
97 createPowerGroupBox();
99 rightLayout->addWidget(positionGroupBox);
100 rightLayout->addWidget(miscGroupBox);
101 rightLayout->addWidget(fsmGroupBox);
102 rightLayout->addWidget(powerGroupBox);
103 rightLayout->addWidget(actuatorsGroupBox);
106 void RobomonAtlantis::createPlaygroundGroupBox()
108 playgroundGroupBox = new QGroupBox(tr("Playground"));
109 QHBoxLayout *layout = new QHBoxLayout();
111 playgroundScene = new PlaygroundScene();
112 playgroundSceneView = new PlaygroundView(playgroundScene);
114 playgroundSceneView->setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers)));
115 playgroundSceneView->setRenderHints(QPainter::Antialiasing);
117 //playgroundSceneView->setMinimumWidth(630);
118 //playgroundSceneView->setMinimumHeight(445);
119 playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
120 playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
121 playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
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 puckInside = new QCheckBox("Puck inside");
184 layout->addWidget(puckInside);
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();
231 createMotorsGroupBox();
233 layout->setAlignment(Qt::AlignLeft);
234 layout->addWidget(enginesGroupBox);
235 actuatorsGroupBox->setLayout(layout);
238 void RobomonAtlantis::createPowerGroupBox()
240 powerGroupBox = new QGroupBox(tr("Power management"));
241 powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
242 QGridLayout *layout = new QGridLayout();
244 voltage33CheckBox = new QCheckBox(tr("&3.3V"));
245 voltage50CheckBox = new QCheckBox(tr("&5.0V"));
246 voltage80CheckBox = new QCheckBox(tr("&8.0V"));
248 voltage33CheckBox->setShortcut(tr("3"));
249 voltage50CheckBox->setShortcut(tr("5"));
250 voltage80CheckBox->setShortcut(tr("8"));
252 layout->addWidget(voltage33CheckBox, 0, 0);
253 layout->addWidget(voltage50CheckBox, 1, 0);
254 layout->addWidget(voltage80CheckBox, 2, 0);
255 layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
257 voltage33LineEdit = new QLineEdit();
258 voltage50LineEdit = new QLineEdit();
259 voltage80LineEdit = new QLineEdit();
260 voltageBATLineEdit = new QLineEdit();
262 voltage33LineEdit->setReadOnly(true);
263 voltage50LineEdit->setReadOnly(true);
264 voltage80LineEdit->setReadOnly(true);
265 voltageBATLineEdit->setReadOnly(true);
267 layout->addWidget(voltage33LineEdit, 0, 1);
268 layout->addWidget(voltage50LineEdit, 1, 1);
269 layout->addWidget(voltage80LineEdit, 2, 1);
270 layout->addWidget(voltageBATLineEdit, 3, 1);
272 powerGroupBox->setLayout(layout);
275 void RobomonAtlantis::createMotorsGroupBox()
277 enginesGroupBox = new QGroupBox(tr("Motors"));
278 QVBoxLayout *layout = new QVBoxLayout();
279 QHBoxLayout *layout1 = new QHBoxLayout();
280 QHBoxLayout *layout2 = new QHBoxLayout();
282 leftMotorSlider = new QSlider(Qt::Vertical);
283 rightMotorSlider = new QSlider(Qt::Vertical);
284 bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
285 stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
287 leftMotorSlider->setMinimum(-100);
288 leftMotorSlider->setMaximum(100);
289 leftMotorSlider->setTracking(false);
290 leftMotorSlider->setTickPosition(QSlider::TicksLeft);
292 rightMotorSlider->setMinimum(-100);
293 rightMotorSlider->setMaximum(100);
294 rightMotorSlider->setTracking(false);
295 rightMotorSlider->setTickPosition(QSlider::TicksRight);
297 stopMotorsPushButton->setMaximumWidth(90);
299 layout1->addWidget(leftMotorSlider);
300 layout1->addWidget(MiscGui::createLabel("0"));
301 layout1->addWidget(rightMotorSlider);
303 layout2->addWidget(bothMotorsCheckBox);
305 layout->addWidget(MiscGui::createLabel("100"));
306 layout->addLayout(layout1);
307 layout->addWidget(MiscGui::createLabel("-100"));
308 layout->addLayout(layout2);
309 layout->addWidget(stopMotorsPushButton);
310 enginesGroupBox->setLayout(layout);
313 void RobomonAtlantis::createRobots()
315 robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
316 robotRefPos->setZValue(11);
317 trailRefPos = new Trail(QPen(Qt::darkBlue));
318 trailRefPos->setZValue(11);
320 robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray));
321 robotEstPosBest->setZValue(10);
322 trailEstPosBest = new Trail(QPen());
323 trailEstPosBest->setZValue(10);
325 robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
326 robotEstPosOdo->setZValue(10);
327 trailOdoPos = new Trail(QPen(Qt::red));
328 trailOdoPos->setZValue(10);
330 robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen));
331 robotEstPosIndepOdo->setZValue(10);
332 trailPosIndepOdo = new Trail(QPen(Qt::green));
333 trailPosIndepOdo->setZValue(10);
335 playgroundScene->addItem(robotRefPos);
336 playgroundScene->addItem(robotEstPosBest);
337 playgroundScene->addItem(robotEstPosIndepOdo);
338 playgroundScene->addItem(robotEstPosOdo);
342 playgroundScene->addItem(trailRefPos);
343 playgroundScene->addItem(trailPosIndepOdo);
344 playgroundScene->addItem(trailOdoPos);
346 hokuyoScan = new HokuyoScan();
347 hokuyoScan->setZValue(10);
348 playgroundScene->addItem(hokuyoScan);
352 /**********************************************************************
354 **********************************************************************/
355 void RobomonAtlantis::createActions()
357 /* power management */
358 connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
359 this, SLOT(setVoltage33(int)));
360 connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
361 this, SLOT(setVoltage50(int)));
362 connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
363 this, SLOT(setVoltage80(int)));
366 connect(leftMotorSlider, SIGNAL(valueChanged(int)),
367 this, SLOT(setLeftMotor(int)));
368 connect(rightMotorSlider, SIGNAL(valueChanged(int)),
369 this, SLOT(setRightMotor(int)));
370 connect(stopMotorsPushButton, SIGNAL(clicked()),
371 this, SLOT(stopMotors()));
373 connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
375 /* obstacle simulation */
376 simulationEnabled = 0;
377 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
378 this, SLOT(setSimulation(int)));
379 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
380 this, SLOT(setObstacleSimulation(int)));
381 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
382 playgroundScene, SLOT(showObstacle(int)));
383 connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
384 this, SLOT(changeObstacle(QPointF)));
387 void RobomonAtlantis::setVoltage33(int state)
390 orte.pwr_ctrl.voltage33 = true;
392 orte.pwr_ctrl.voltage33 = false;
395 void RobomonAtlantis::setVoltage50(int state)
398 orte.pwr_ctrl.voltage50 = true;
400 orte.pwr_ctrl.voltage50 = false;
403 void RobomonAtlantis::setVoltage80(int state)
406 orte.pwr_ctrl.voltage80 = true;
408 orte.pwr_ctrl.voltage80 = false;
411 void RobomonAtlantis::setLeftMotor(int value)
414 short int rightMotor;
416 if(bothMotorsCheckBox->isChecked())
417 rightMotorSlider->setValue(value);
419 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
420 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
422 orte.motion_speed.left = leftMotor;
423 orte.motion_speed.right = rightMotor;
427 void RobomonAtlantis::setRightMotor(int value)
430 short int rightMotor;
432 if(bothMotorsCheckBox->isChecked())
433 leftMotorSlider->setValue(value);
435 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
436 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
438 orte.motion_speed.left = leftMotor;
439 orte.motion_speed.right = rightMotor;
443 void RobomonAtlantis::stopMotors()
445 leftMotorSlider->setValue(0);
446 rightMotorSlider->setValue(0);
449 void RobomonAtlantis::showMap(bool show)
453 if (sharedMemoryOpened == false)
457 mapTimer = new QTimer(this);
458 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
459 mapTimer->start(200);
461 if(mapTimer != NULL) {
463 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
466 playgroundScene->showMap(show);
469 void RobomonAtlantis::paintMap()
472 struct map *map = ShmapIsMapInit();
476 for(int i=0; i < MAP_WIDTH; i++) {
477 for(int j=0; j<MAP_HEIGHT; j++) {
480 struct map_cell *cell = &map->cells[j][i];
483 if ((cell->flags & MAP_FLAG_WALL) &&
484 (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
486 if (cell->flags & MAP_FLAG_IGNORE_OBST)
488 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
490 if (cell->flags & MAP_FLAG_PATH)
492 if (cell->flags & MAP_FLAG_START)
494 if (cell->flags & MAP_FLAG_GOAL)
496 if (cell->flags & MAP_FLAG_PLAN_MARGIN) {
497 QColor c(240, 170, 50); /* orange */
500 if (cell->detected_obstacle) {
501 QColor c1(color), c2(blue);
502 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
503 QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
504 c1.green() + (int)(f*(c2.green() - c1.green())),
505 c1.blue() + (int)(f*(c2.blue() - c1.blue())));
508 if (cell->flags & MAP_FLAG_DET_OBST)
511 playgroundScene->setMapColor(i, j, color);
516 void RobomonAtlantis::setSimulation(int state)
519 robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
521 if (!simulationEnabled)
523 robottype_publisher_hokuyo_scan_destroy(&orte);
525 simulationEnabled = state;
529 \fn RobomonAtlantis::setObstacleSimulation(int state)
531 void RobomonAtlantis::setObstacleSimulation(int state)
534 /* TODO Maybe it is possible to attach only once to Shmap */
536 obstacleSimulationTimer = new QTimer(this);
537 connect(obstacleSimulationTimer, SIGNAL(timeout()),
538 this, SLOT(simulateObstaclesHokuyo()));
539 obstacleSimulationTimer->start(100);
540 setMouseTracking(true);
542 if (obstacleSimulationTimer)
543 delete obstacleSimulationTimer;
544 //double distance = 0.8;
549 void RobomonAtlantis::simulateObstaclesHokuyo()
551 double distance, wall_distance;
553 uint16_t *hokuyo = orte.hokuyo_scan.data;
555 for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
556 wall_distance = distanceToWallHokuyo(i);
557 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
558 if (wall_distance < distance)
559 distance = wall_distance;
560 hokuyo[i] = distance*1000;
562 ORTEPublicationSend(orte.publication_hokuyo_scan);
566 void RobomonAtlantis::changeObstacle(QPointF position)
568 if (!simulationEnabled) {
569 simulationEnabled = 1;
570 obstacleSimulationCheckBox->setChecked(true);
573 simulatedObstacle.x = position.x();
574 simulatedObstacle.y = position.y();
575 simulateObstaclesHokuyo();
578 /**********************************************************************
580 **********************************************************************/
581 bool RobomonAtlantis::event(QEvent *event)
583 switch (event->type()) {
584 case QEVENT(QEV_MOTION_STATUS):
585 emit motionStatusReceivedSignal();
587 case QEVENT(QEV_HOKUYO_SCAN):
588 hokuyoScan->newScan(&orte.hokuyo_scan);
590 case QEVENT(QEV_REFERENCE_POSITION):
591 emit actualPositionReceivedSignal();
593 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
594 estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
595 estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
596 estPosPhi->setText(QString("%1(%2)")
597 .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
598 .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
599 robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
600 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
601 trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
602 orte.est_pos_indep_odo.y));
604 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
605 robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
606 orte.est_pos_odo.y, orte.est_pos_odo.phi);
607 trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
608 orte.est_pos_odo.y));
610 case QEVENT(QEV_ESTIMATED_POSITION_BEST):
611 robotEstPosBest->moveRobot(orte.est_pos_best.x,
612 orte.est_pos_best.y, orte.est_pos_best.phi);
613 trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x,
614 orte.est_pos_best.y));
615 hokuyoScan->setPosition(orte.est_pos_best.x,
617 orte.est_pos_best.phi);
619 case QEVENT(QEV_POWER_VOLTAGE):
620 emit powerVoltageReceivedSignal();
622 case QEVENT(QEV_FSM_MAIN):
623 fsm_main_state->setText(orte.fsm_main.state_name);
625 case QEVENT(QEV_FSM_ACT):
626 fsm_act_state->setText(orte.fsm_act.state_name);
628 case QEVENT(QEV_FSM_MOTION):
629 fsm_motion_state->setText(orte.fsm_motion.state_name);
632 if (event->type() == QEvent::Close)
633 closeEvent((QCloseEvent *)event);
634 else if (event->type() == QEvent::KeyPress)
635 keyPressEvent((QKeyEvent *)event);
636 else if (event->type() == QEvent::KeyRelease)
637 keyReleaseEvent((QKeyEvent *)event);
638 else if (event->type() == QEvent::FocusIn)
640 else if (event->type() == QEvent::FocusOut)
652 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
656 if (event->isAutoRepeat()) {
657 switch (event->key()) {
659 peak = leftMotorSlider->minimum()/2;
660 if (leftMotorValue < peak ||
661 rightMotorValue < peak)
665 leftMotorValue *= gain;
666 rightMotorValue *= gain;
667 leftMotorSlider->setValue((int)leftMotorValue);
668 rightMotorSlider->setValue((int)rightMotorValue);
674 peak = leftMotorSlider->maximum()/2;
675 if (leftMotorValue > peak ||
676 rightMotorValue > peak)
680 leftMotorValue *= gain;
681 rightMotorValue *= gain;
682 leftMotorSlider->setValue((int)leftMotorValue);
683 rightMotorSlider->setValue((int)rightMotorValue);
693 switch (event->key()) {
697 bothMotorsCheckBox->setChecked(true);
698 leftMotorSlider->setValue((int)leftMotorValue);
699 setLeftMotor((int)leftMotorValue);
703 rightMotorValue = -1;
704 bothMotorsCheckBox->setChecked(true);
705 leftMotorSlider->setValue((int)leftMotorValue);
706 setLeftMotor((int)leftMotorValue);
711 leftMotorSlider->setValue((int)leftMotorValue);
712 rightMotorSlider->setValue((int)rightMotorValue);
713 setLeftMotor((int)leftMotorValue);
714 setRightMotor((int)leftMotorValue);
718 rightMotorValue = -1;
719 leftMotorSlider->setValue((int)leftMotorValue);
720 rightMotorSlider->setValue((int)rightMotorValue);
721 setLeftMotor((int)leftMotorValue);
722 setRightMotor((int)rightMotorValue);
731 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
733 if (event->isAutoRepeat()) {
738 switch (event->key()) {
745 bothMotorsCheckBox->setChecked(false);
746 leftMotorSlider->setValue((int)leftMotorValue);
747 rightMotorSlider->setValue((int)rightMotorValue);
756 void RobomonAtlantis::closeEvent(QCloseEvent *)
758 robottype_roboorte_destroy(&orte);
761 /**********************************************************************
763 **********************************************************************/
764 void RobomonAtlantis::createOrte()
770 rv = robottype_roboorte_init(&orte);
772 printf("RobomonAtlantis: Unable to initialize ORTE\n");
776 robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
778 robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
779 robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
782 robottype_subscriber_pwr_voltage_create(&orte,
783 receivePowerVoltageCallBack, this);
784 robottype_subscriber_motion_status_create(&orte,
785 receiveMotionStatusCallBack, this);
786 robottype_subscriber_ref_pos_create(&orte,
787 receiveActualPositionCallBack, this);
788 robottype_subscriber_est_pos_odo_create(&orte,
789 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
790 robottype_subscriber_est_pos_indep_odo_create(&orte,
791 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
792 robottype_subscriber_est_pos_best_create(&orte,
793 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
794 robottype_subscriber_hokuyo_scan_create(&orte,
795 generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
796 robottype_subscriber_fsm_main_create(&orte,
797 rcv_fsm_main_cb, this);
798 robottype_subscriber_fsm_motion_create(&orte,
799 rcv_fsm_motion_cb, this);
800 robottype_subscriber_fsm_act_create(&orte,
801 rcv_fsm_act_cb, this);
804 orte.motion_speed.left = 0;
805 orte.motion_speed.right = 0;
807 /* power management */
808 orte.pwr_ctrl.voltage33 = true;
809 orte.pwr_ctrl.voltage50 = true;
810 orte.pwr_ctrl.voltage80 = true;
811 voltage33CheckBox->setChecked(true);
812 voltage50CheckBox->setChecked(true);
813 voltage80CheckBox->setChecked(true);
817 /* set actions to do when we receive data from orte */
818 connect(this, SIGNAL(motionStatusReceivedSignal()),
819 this, SLOT(motionStatusReceived()));
820 connect(this, SIGNAL(actualPositionReceivedSignal()),
821 this, SLOT(actualPositionReceived()));
822 connect(this, SIGNAL(powerVoltageReceivedSignal()),
823 this, SLOT(powerVoltageReceived()));
826 void RobomonAtlantis::motionStatusReceived()
828 WDBG("ORTE received: motion status");
831 void RobomonAtlantis::actualPositionReceived()
833 actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
834 actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
835 actPosPhi->setText(QString("%1(%2)")
836 .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
837 .arg(orte.ref_pos.phi, 0, 'f', 1));
838 robotRefPos->moveRobot(orte.ref_pos.x,
839 orte.ref_pos.y, orte.ref_pos.phi);
840 trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
843 void RobomonAtlantis::powerVoltageReceived()
845 voltage33LineEdit->setText(QString("%1").arg(
846 orte.pwr_voltage.voltage33, 0, 'f', 3));
847 voltage50LineEdit->setText(QString("%1").arg(
848 orte.pwr_voltage.voltage50, 0, 'f', 3));
849 voltage80LineEdit->setText(QString("%1").arg(
850 orte.pwr_voltage.voltage80, 0, 'f', 3));
851 voltageBATLineEdit->setText(QString("%1").arg(
852 orte.pwr_voltage.voltageBAT, 0, 'f', 3));
856 /**********************************************************************
858 **********************************************************************/
859 void RobomonAtlantis::openSharedMemory()
862 int sharedSegmentSize;
864 if (sharedMemoryOpened)
867 sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
869 /* Get segment identificator in a read only mode */
870 segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
871 if(segmentId == -1) {
872 QMessageBox::critical(this, "robomon",
873 "Unable to open shared memory segment!");
880 /* Attach the shared memory segment */
881 //map = (_Map*)shmat (segmentId, (void*) 0, 0);
883 sharedMemoryOpened = true;
886 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
888 double distance=4.0, min_distance=4.0;
891 struct map *map = ShmapIsMapInit();
893 if (!map) return min_distance;
895 // Simulate obstacles
896 for(j=0;j<MAP_HEIGHT;j++) {
897 for (i=0;i<MAP_WIDTH;i++) {
898 struct map_cell *cell = &map->cells[j][i];
899 if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
901 ShmapCell2Point(i, j, &wall.x, &wall.y);
903 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
904 if (distance<min_distance) min_distance = distance;
913 * Calculation for Hokuyo simulation. Calculates distance that would
914 * be returned by Hokuyo sensors, if there is only one obstacle (as
915 * specified by parameters).
917 * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
918 * @param obstacle Position of the obstacle (x, y in meters).
919 * @param obstacleSize Size (diameter) of the obstacle in meters.
921 * @return Distance measured by sensors in meters.
923 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
925 struct robot_pos_type e = orte.est_pos_best;
929 s.x = HOKUYO_CENTER_OFFSET_M;
931 s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
933 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
934 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
935 sensor_a = e.phi + s.ang;
937 const double sensorRange = 4.0; /*[meters]*/
939 double distance, angle;
941 angle = sensor.angleTo(obstacle) - sensor_a;
942 angle = fmod(angle, 2.0*M_PI);
943 if (angle > +M_PI) angle -= 2.0*M_PI;
944 if (angle < -M_PI) angle += 2.0*M_PI;
946 distance = sensor.distanceTo(obstacle) - obstacleSize/2.0;
947 if (angle < atan(obstacleSize/2.0 / distance)) {
948 // We can see the obstackle from here.
949 if (angle < M_PI/2.0) {
950 distance = distance/cos(angle);
952 if (distance > sensorRange)
953 distance = sensorRange;
955 distance = sensorRange;
961 void RobomonAtlantis::sendStart(int plug)
963 orte.robot_cmd.start = plug ? 0 : 1;
964 ORTEPublicationSend(orte.publication_robot_cmd);
967 void RobomonAtlantis::resetTrails()
969 trailRefPos->reset();
970 trailEstPosBest->reset();
971 trailPosIndepOdo->reset();
972 trailOdoPos->reset();
975 void RobomonAtlantis::showTrails(bool show)
977 trailRefPos->setVisible(show && robotRefPos->isVisible());
978 trailEstPosBest->setVisible(show && robotEstPosBest->isVisible());
979 trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible());
980 trailOdoPos->setVisible(show && robotEstPosOdo->isVisible());