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();
91 QGridLayout *layout = new QGridLayout();
92 QVBoxLayout *vlayout = new QVBoxLayout();
93 //vlayout->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
95 createPositionGroupBox();
98 createActuatorsGroupBox();
99 createPowerGroupBox();
101 vlayout->addWidget(positionGroupBox);
102 vlayout->addWidget(miscGroupBox);
103 vlayout->addWidget(fsmGroupBox);
104 vlayout->addWidget(powerGroupBox);
105 layout->addLayout(vlayout, 0, 0);
106 layout->addWidget(actuatorsGroupBox, 0, 1);
108 rightLayout->addLayout(layout);
111 void RobomonAtlantis::createPlaygroundGroupBox()
113 playgroundGroupBox = new QGroupBox(tr("Playground"));
114 QHBoxLayout *layout = new QHBoxLayout();
116 playgroundScene = new PlaygroundScene();
117 playgroundSceneView = new PlaygroundView(playgroundScene);
118 //playgroundSceneView->setMinimumWidth(630);
119 //playgroundSceneView->setMinimumHeight(445);
120 playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
121 playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
122 playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
123 layout->addWidget(playgroundSceneView);
125 playgroundGroupBox->setLayout(layout);
128 void RobomonAtlantis::createPositionGroupBox()
130 positionGroupBox = new QGroupBox(tr("Position state"));
131 positionGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
132 QGridLayout *layout = new QGridLayout();
134 actPosX = new QLineEdit();
135 actPosY = new QLineEdit();
136 actPosPhi = new QLineEdit();
138 estPosX = new QLineEdit();
139 estPosY = new QLineEdit();
140 estPosPhi = new QLineEdit();
142 actPosX->setReadOnly(true);
143 actPosY->setReadOnly(true);
144 actPosPhi->setReadOnly(true);
146 estPosX->setReadOnly(true);
147 estPosY->setReadOnly(true);
148 estPosPhi->setReadOnly(true);
150 layout->addWidget(MiscGui::createLabel("X"), 1, 0);
151 layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
152 layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
154 layout->addWidget(MiscGui::createLabel("X"), 5, 0);
155 layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
156 layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
158 layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
159 layout->addWidget(actPosX, 1, 1);
160 layout->addWidget(actPosY, 2, 1);
161 layout->addWidget(actPosPhi, 3, 1);
163 layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
164 layout->addWidget(estPosX, 5, 1);
165 layout->addWidget(estPosY, 6, 1);
166 layout->addWidget(estPosPhi, 7, 1);
168 positionGroupBox->setLayout(layout);
171 void RobomonAtlantis::createMiscGroupBox()
173 miscGroupBox = new QGroupBox(tr("Miscellaneous"));
174 miscGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
175 QGridLayout *layout = new QGridLayout();
177 obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
178 obstacleSimulationCheckBox->setShortcut(tr("o"));
179 layout->addWidget(obstacleSimulationCheckBox);
181 startPlug = new QCheckBox("Start plug");
182 layout->addWidget(startPlug);
184 puckInside = new QCheckBox("Puck inside");
185 layout->addWidget(puckInside);
187 miscGroupBox->setLayout(layout);
190 void RobomonAtlantis::createFSMGroupBox()
192 fsmGroupBox = new QGroupBox(tr("FSM"));
193 fsmGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
194 QGridLayout *layout = new QGridLayout();
196 layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
197 fsm_main_state = new QLabel();
198 fsm_main_state->setMinimumWidth(100);
199 fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
200 layout->addWidget(fsm_main_state, 1, 1);
202 layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
203 fsm_act_state = new QLabel();
204 layout->addWidget(fsm_act_state, 2, 1);
206 layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
207 fsm_motion_state = new QLabel();
208 layout->addWidget(fsm_motion_state, 3, 1);
210 fsmGroupBox->setLayout(layout);
213 void RobomonAtlantis::createDebugGroupBox()
215 debugGroupBox = new QGroupBox(tr("Debug window"));
216 debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
217 QHBoxLayout *layout = new QHBoxLayout();
219 debugWindow = new QTextEdit();
220 debugWindow->setReadOnly(true);
222 layout->addWidget(debugWindow);
223 debugGroupBox->setLayout(layout);
226 void RobomonAtlantis::createActuatorsGroupBox()
228 actuatorsGroupBox = new QGroupBox(tr("Actuators"));
229 actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
230 QHBoxLayout *layout = new QHBoxLayout();
232 createMotorsGroupBox();
234 layout->setAlignment(Qt::AlignLeft);
235 layout->addWidget(enginesGroupBox);
236 actuatorsGroupBox->setLayout(layout);
239 void RobomonAtlantis::createPowerGroupBox()
241 powerGroupBox = new QGroupBox(tr("Power management"));
242 powerGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
243 QGridLayout *layout = new QGridLayout();
245 voltage33CheckBox = new QCheckBox(tr("&3.3V"));
246 voltage50CheckBox = new QCheckBox(tr("&5.0V"));
247 voltage80CheckBox = new QCheckBox(tr("&8.0V"));
249 voltage33CheckBox->setShortcut(tr("3"));
250 voltage50CheckBox->setShortcut(tr("5"));
251 voltage80CheckBox->setShortcut(tr("8"));
253 layout->addWidget(voltage33CheckBox, 0, 0);
254 layout->addWidget(voltage50CheckBox, 1, 0);
255 layout->addWidget(voltage80CheckBox, 2, 0);
256 layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
258 voltage33LineEdit = new QLineEdit();
259 voltage50LineEdit = new QLineEdit();
260 voltage80LineEdit = new QLineEdit();
261 voltageBATLineEdit = new QLineEdit();
263 voltage33LineEdit->setReadOnly(true);
264 voltage50LineEdit->setReadOnly(true);
265 voltage80LineEdit->setReadOnly(true);
266 voltageBATLineEdit->setReadOnly(true);
268 layout->addWidget(voltage33LineEdit, 0, 1);
269 layout->addWidget(voltage50LineEdit, 1, 1);
270 layout->addWidget(voltage80LineEdit, 2, 1);
271 layout->addWidget(voltageBATLineEdit, 3, 1);
273 powerGroupBox->setLayout(layout);
276 void RobomonAtlantis::createMotorsGroupBox()
278 enginesGroupBox = new QGroupBox(tr("Motors"));
279 QVBoxLayout *layout = new QVBoxLayout();
280 QHBoxLayout *layout1 = new QHBoxLayout();
281 QHBoxLayout *layout2 = new QHBoxLayout();
283 leftMotorSlider = new QSlider(Qt::Vertical);
284 rightMotorSlider = new QSlider(Qt::Vertical);
285 bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
286 stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
288 leftMotorSlider->setMinimum(-100);
289 leftMotorSlider->setMaximum(100);
290 leftMotorSlider->setTracking(false);
291 leftMotorSlider->setTickPosition(QSlider::TicksLeft);
293 rightMotorSlider->setMinimum(-100);
294 rightMotorSlider->setMaximum(100);
295 rightMotorSlider->setTracking(false);
296 rightMotorSlider->setTickPosition(QSlider::TicksRight);
298 stopMotorsPushButton->setMaximumWidth(90);
300 layout1->addWidget(leftMotorSlider);
301 layout1->addWidget(MiscGui::createLabel("0"));
302 layout1->addWidget(rightMotorSlider);
304 layout2->addWidget(bothMotorsCheckBox);
306 layout->addWidget(MiscGui::createLabel("100"));
307 layout->addLayout(layout1);
308 layout->addWidget(MiscGui::createLabel("-100"));
309 layout->addLayout(layout2);
310 layout->addWidget(stopMotorsPushButton);
311 enginesGroupBox->setLayout(layout);
314 void RobomonAtlantis::createRobots()
316 robotActPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
317 robotActPos->setZValue(11);
318 trailRefPos = new Trail(QPen(Qt::darkBlue));
319 trailRefPos->setZValue(11);
321 robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
322 robotEstPosOdo->setZValue(10);
323 trailOdoPos = new Trail(QPen(Qt::white));
324 trailOdoPos->setZValue(10);
325 robotEstPosIndepOdo = new Robot("Odo", QPen(), QBrush(Qt::darkGray));
326 robotEstPosIndepOdo->setZValue(10);
327 trailPosIndepOdo = new Trail(QPen());
328 trailPosIndepOdo->setZValue(10);
330 playgroundScene->addItem(robotActPos);
331 playgroundScene->addItem(robotEstPosIndepOdo);
332 playgroundScene->addItem(robotEstPosOdo);
336 playgroundScene->addItem(trailRefPos);
337 playgroundScene->addItem(trailPosIndepOdo);
338 playgroundScene->addItem(trailOdoPos);
341 /**********************************************************************
343 **********************************************************************/
344 void RobomonAtlantis::createActions()
346 /* power management */
347 connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
348 this, SLOT(setVoltage33(int)));
349 connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
350 this, SLOT(setVoltage50(int)));
351 connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
352 this, SLOT(setVoltage80(int)));
355 connect(leftMotorSlider, SIGNAL(valueChanged(int)),
356 this, SLOT(setLeftMotor(int)));
357 connect(rightMotorSlider, SIGNAL(valueChanged(int)),
358 this, SLOT(setRightMotor(int)));
359 connect(stopMotorsPushButton, SIGNAL(clicked()),
360 this, SLOT(stopMotors()));
362 connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
364 /* obstacle simulation */
365 simulationEnabled = 0;
366 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
367 this, SLOT(setSimulation(int)));
368 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
369 this, SLOT(setObstacleSimulation(int)));
370 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
371 playgroundScene, SLOT(showObstacle(int)));
372 connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
373 this, SLOT(changeObstacle(QPointF)));
376 void RobomonAtlantis::setVoltage33(int state)
379 orte.pwr_ctrl.voltage33 = true;
381 orte.pwr_ctrl.voltage33 = false;
384 void RobomonAtlantis::setVoltage50(int state)
387 orte.pwr_ctrl.voltage50 = true;
389 orte.pwr_ctrl.voltage50 = false;
392 void RobomonAtlantis::setVoltage80(int state)
395 orte.pwr_ctrl.voltage80 = true;
397 orte.pwr_ctrl.voltage80 = false;
400 void RobomonAtlantis::setLeftMotor(int value)
403 short int rightMotor;
405 if(bothMotorsCheckBox->isChecked())
406 rightMotorSlider->setValue(value);
408 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
409 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
411 orte.motion_speed.left = leftMotor;
412 orte.motion_speed.right = rightMotor;
416 void RobomonAtlantis::setRightMotor(int value)
419 short int rightMotor;
421 if(bothMotorsCheckBox->isChecked())
422 leftMotorSlider->setValue(value);
424 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
425 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
427 orte.motion_speed.left = leftMotor;
428 orte.motion_speed.right = rightMotor;
432 void RobomonAtlantis::stopMotors()
434 leftMotorSlider->setValue(0);
435 rightMotorSlider->setValue(0);
438 void RobomonAtlantis::showMap(bool show)
442 if (sharedMemoryOpened == false)
446 mapTimer = new QTimer(this);
447 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
448 mapTimer->start(200);
451 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
453 playgroundScene->showMap(show);
456 void RobomonAtlantis::paintMap()
459 struct map *map = ShmapIsMapInit();
463 for(int i=0; i < MAP_WIDTH; i++) {
464 for(int j=0; j<MAP_HEIGHT; j++) {
467 struct map_cell *cell = &map->cells[j][i];
470 if ((cell->flags & MAP_FLAG_WALL) &&
471 (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
473 if (cell->flags & MAP_FLAG_IGNORE_OBST)
475 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
477 if (cell->flags & MAP_FLAG_PATH)
479 if (cell->flags & MAP_FLAG_START)
481 if (cell->flags & MAP_FLAG_GOAL)
483 if (cell->detected_obstacle) {
484 QColor c1(color), c2(blue);
485 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
486 QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
487 c1.green() + (int)(f*(c2.green() - c1.green())),
488 c1.blue() + (int)(f*(c2.blue() - c1.blue())));
491 if (cell->flags & MAP_FLAG_DET_OBST)
494 playgroundScene->setMapColor(i, j, color);
499 void RobomonAtlantis::setSimulation(int state)
502 robottype_publisher_hokuyo_scan_create(&orte,
503 dummy_publisher_callback, this);
505 if (!simulationEnabled)
507 robottype_publisher_hokuyo_scan_destroy(&orte);
509 simulationEnabled = state;
513 \fn RobomonAtlantis::setObstacleSimulation(int state)
515 void RobomonAtlantis::setObstacleSimulation(int state)
518 /* TODO Maybe it is possible to attach only once to Shmap */
520 obstacleSimulationTimer = new QTimer(this);
521 connect(obstacleSimulationTimer, SIGNAL(timeout()),
522 this, SLOT(simulateObstaclesHokuyo()));
523 obstacleSimulationTimer->start(100);
524 setMouseTracking(true);
526 if (obstacleSimulationTimer)
527 delete obstacleSimulationTimer;
528 //double distance = 0.8;
533 void RobomonAtlantis::simulateObstaclesHokuyo()
535 double distance, wall_distance;
537 uint16_t *hokuyo = orte.hokuyo_scan.data;
539 for (i=0; i<HOKUYO_CLUSTER_CNT; i++) {
540 wall_distance = distanceToWallHokuyo(i);
541 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
542 if (wall_distance < distance)
543 distance = wall_distance;
544 hokuyo[i] = distance*1000;
546 ORTEPublicationSend(orte.publication_hokuyo_scan);
550 void RobomonAtlantis::changeObstacle(QPointF position)
552 if (!simulationEnabled) {
553 simulationEnabled = 1;
554 obstacleSimulationCheckBox->setChecked(true);
557 simulatedObstacle.x = position.x();
558 simulatedObstacle.y = position.y();
559 simulateObstaclesHokuyo();
562 /**********************************************************************
564 **********************************************************************/
565 bool RobomonAtlantis::event(QEvent *event)
567 switch (event->type()) {
568 case QEVENT(QEV_MOTION_STATUS):
569 emit motionStatusReceivedSignal();
571 case QEVENT(QEV_REFERENCE_POSITION):
572 emit actualPositionReceivedSignal();
574 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
575 estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
576 estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
577 estPosPhi->setText(QString("%1(%2)")
578 .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
579 .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
580 robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
581 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
582 trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
583 orte.est_pos_indep_odo.y));
585 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
586 robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
587 orte.est_pos_odo.y, orte.est_pos_odo.phi);
588 trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
589 orte.est_pos_odo.y));
591 case QEVENT(QEV_POWER_VOLTAGE):
592 emit powerVoltageReceivedSignal();
594 case QEVENT(QEV_FSM_MAIN):
595 fsm_main_state->setText(orte.fsm_main.state_name);
597 case QEVENT(QEV_FSM_ACT):
598 fsm_act_state->setText(orte.fsm_act.state_name);
600 case QEVENT(QEV_FSM_MOTION):
601 fsm_motion_state->setText(orte.fsm_motion.state_name);
604 if (event->type() == QEvent::Close)
605 closeEvent((QCloseEvent *)event);
606 else if (event->type() == QEvent::KeyPress)
607 keyPressEvent((QKeyEvent *)event);
608 else if (event->type() == QEvent::KeyRelease)
609 keyReleaseEvent((QKeyEvent *)event);
610 else if (event->type() == QEvent::FocusIn)
612 else if (event->type() == QEvent::FocusOut)
624 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
628 if (event->isAutoRepeat()) {
629 switch (event->key()) {
631 peak = leftMotorSlider->minimum()/2;
632 if (leftMotorValue < peak ||
633 rightMotorValue < peak)
637 leftMotorValue *= gain;
638 rightMotorValue *= gain;
639 leftMotorSlider->setValue((int)leftMotorValue);
640 rightMotorSlider->setValue((int)rightMotorValue);
646 peak = leftMotorSlider->maximum()/2;
647 if (leftMotorValue > peak ||
648 rightMotorValue > peak)
652 leftMotorValue *= gain;
653 rightMotorValue *= gain;
654 leftMotorSlider->setValue((int)leftMotorValue);
655 rightMotorSlider->setValue((int)rightMotorValue);
665 switch (event->key()) {
669 bothMotorsCheckBox->setChecked(true);
670 leftMotorSlider->setValue((int)leftMotorValue);
671 setLeftMotor((int)leftMotorValue);
675 rightMotorValue = -1;
676 bothMotorsCheckBox->setChecked(true);
677 leftMotorSlider->setValue((int)leftMotorValue);
678 setLeftMotor((int)leftMotorValue);
683 leftMotorSlider->setValue((int)leftMotorValue);
684 rightMotorSlider->setValue((int)rightMotorValue);
685 setLeftMotor((int)leftMotorValue);
686 setRightMotor((int)leftMotorValue);
690 rightMotorValue = -1;
691 leftMotorSlider->setValue((int)leftMotorValue);
692 rightMotorSlider->setValue((int)rightMotorValue);
693 setLeftMotor((int)leftMotorValue);
694 setRightMotor((int)rightMotorValue);
703 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
705 if (event->isAutoRepeat()) {
710 switch (event->key()) {
717 bothMotorsCheckBox->setChecked(false);
718 leftMotorSlider->setValue((int)leftMotorValue);
719 rightMotorSlider->setValue((int)rightMotorValue);
728 void RobomonAtlantis::closeEvent(QCloseEvent *)
730 robottype_roboorte_destroy(&orte);
733 /**********************************************************************
735 **********************************************************************/
736 void RobomonAtlantis::createOrte()
742 rv = robottype_roboorte_init(&orte);
744 printf("RobomonAtlantis: Unable to initialize ORTE\n");
748 robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
750 robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
751 robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
754 robottype_subscriber_pwr_voltage_create(&orte,
755 receivePowerVoltageCallBack, this);
756 robottype_subscriber_motion_status_create(&orte,
757 receiveMotionStatusCallBack, this);
758 robottype_subscriber_ref_pos_create(&orte,
759 receiveActualPositionCallBack, this);
760 robottype_subscriber_est_pos_odo_create(&orte,
761 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
762 robottype_subscriber_est_pos_indep_odo_create(&orte,
763 generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
764 robottype_subscriber_fsm_main_create(&orte,
765 rcv_fsm_main_cb, this);
766 robottype_subscriber_fsm_motion_create(&orte,
767 rcv_fsm_motion_cb, this);
768 robottype_subscriber_fsm_act_create(&orte,
769 rcv_fsm_act_cb, this);
772 orte.motion_speed.left = 0;
773 orte.motion_speed.right = 0;
775 /* power management */
776 orte.pwr_ctrl.voltage33 = true;
777 orte.pwr_ctrl.voltage50 = true;
778 orte.pwr_ctrl.voltage80 = true;
779 voltage33CheckBox->setChecked(true);
780 voltage50CheckBox->setChecked(true);
781 voltage80CheckBox->setChecked(true);
785 /* set actions to do when we receive data from orte */
786 connect(this, SIGNAL(motionStatusReceivedSignal()),
787 this, SLOT(motionStatusReceived()));
788 connect(this, SIGNAL(actualPositionReceivedSignal()),
789 this, SLOT(actualPositionReceived()));
790 connect(this, SIGNAL(powerVoltageReceivedSignal()),
791 this, SLOT(powerVoltageReceived()));
794 void RobomonAtlantis::motionStatusReceived()
796 WDBG("ORTE received: motion status");
799 void RobomonAtlantis::actualPositionReceived()
801 actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
802 actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
803 actPosPhi->setText(QString("%1(%2)")
804 .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
805 .arg(orte.ref_pos.phi, 0, 'f', 1));
806 robotActPos->moveRobot(orte.ref_pos.x,
807 orte.ref_pos.y, orte.ref_pos.phi);
808 trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
811 void RobomonAtlantis::powerVoltageReceived()
813 voltage33LineEdit->setText(QString("%1").arg(
814 orte.pwr_voltage.voltage33, 0, 'f', 3));
815 voltage50LineEdit->setText(QString("%1").arg(
816 orte.pwr_voltage.voltage50, 0, 'f', 3));
817 voltage80LineEdit->setText(QString("%1").arg(
818 orte.pwr_voltage.voltage80, 0, 'f', 3));
819 voltageBATLineEdit->setText(QString("%1").arg(
820 orte.pwr_voltage.voltageBAT, 0, 'f', 3));
824 /**********************************************************************
826 **********************************************************************/
827 void RobomonAtlantis::openSharedMemory()
830 int sharedSegmentSize;
832 if (sharedMemoryOpened)
835 sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
837 /* Get segment identificator in a read only mode */
838 segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
839 if(segmentId == -1) {
840 QMessageBox::critical(this, "robomon",
841 "Unable to open shared memory segment!");
848 /* Attach the shared memory segment */
849 //map = (_Map*)shmat (segmentId, (void*) 0, 0);
851 sharedMemoryOpened = true;
854 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
856 double distance=4.0, min_distance=4.0;
859 struct map *map = ShmapIsMapInit();
861 if (!map) return min_distance;
863 // Simulate obstacles
864 for(j=0;j<MAP_HEIGHT;j++) {
865 for (i=0;i<MAP_WIDTH;i++) {
866 struct map_cell *cell = &map->cells[j][i];
867 if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
869 ShmapCell2Point(i, j, &wall.x, &wall.y);
871 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
872 if (distance<min_distance) min_distance = distance;
881 * Calculation for Hokuyo simulation. Calculates distance that would
882 * be returned by Hokuyo sensors, if there is only one obstacle (as
883 * specified by parameters).
885 * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
886 * @param obstacle Position of the obstacle (x, y in meters).
887 * @param obstacleSize Size (diameter) of the obstacle in meters.
889 * @return Distance measured by sensors in meters.
891 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
893 struct robot_pos_type e = orte.est_pos_indep_odo;
897 s.x = HOKUYO_CENTER_OFFSET_M;
899 s.ang = HOKUYO_CLUSTER_TO_RAD(beamnum);
901 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
902 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
903 sensor_a = e.phi + s.ang;
905 const double sensorRange = 4.0; /*[meters]*/
907 double distance, angle;
909 angle = sensor.angleTo(obstacle) - sensor_a;
910 angle = fmod(angle, 2.0*M_PI);
911 if (angle > +M_PI) angle -= 2.0*M_PI;
912 if (angle < -M_PI) angle += 2.0*M_PI;
914 distance = sensor.distanceTo(obstacle)-0.11;
915 if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
916 // We can see the obstackle from here.
917 if (angle < M_PI/2.0) {
918 distance = distance/cos(angle);
920 if (distance > sensorRange)
921 distance = sensorRange;
923 distance = sensorRange;
929 void RobomonAtlantis::sendStart(int plug)
931 orte.robot_cmd.start = plug ? 0 : 1;
932 ORTEPublicationSend(orte.publication_robot_cmd);
935 void RobomonAtlantis::resetTrails()
937 trailRefPos->reset();
938 trailPosIndepOdo->reset();
939 trailOdoPos->reset();
942 void RobomonAtlantis::showTrails(bool show)
944 trailRefPos->setVisible(show);
945 trailPosIndepOdo->setVisible(show);
946 trailOdoPos->setVisible(show);