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"
41 #include <QCoreApplication>
45 #include <QMessageBox>
47 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
54 debugWindowEnabled = false;
59 QHBoxLayout *mainLayout = new QHBoxLayout;
60 mainLayout->addLayout(leftLayout);
61 mainLayout->addLayout(rightLayout);
62 setLayout(mainLayout);
68 setFocusPolicy(Qt::StrongFocus);
69 sharedMemoryOpened = false;
70 WDBG("Youuuhouuuu!!");
73 /**********************************************************************
75 **********************************************************************/
76 void RobomonAtlantis::createLeftLayout()
78 leftLayout = new QVBoxLayout();
80 createDebugGroupBox();
81 debugWindowEnabled = true;
82 createPlaygroundGroupBox();
83 leftLayout->addWidget(playgroundGroupBox);
84 leftLayout->addWidget(debugGroupBox);
87 void RobomonAtlantis::createRightLayout()
89 rightLayout = new QVBoxLayout();
90 QGridLayout *layout = new QGridLayout();
91 QVBoxLayout *vlayout = new QVBoxLayout();
93 createPositionGroupBox();
96 createActuatorsGroupBox();
98 createPowerGroupBox();
99 createSensorsGroupBox();
101 vlayout->addWidget(positionGroupBox);
102 vlayout->addWidget(miscGroupBox);
103 vlayout->addWidget(fsmGroupBox);
104 layout->addLayout(vlayout, 0, 0);
105 layout->addWidget(actuatorsGroupBox, 0, 1);
106 // layout->addWidget(dioGroupBox, 0, 2);
108 rightLayout->addLayout(layout);
109 rightLayout->addWidget(powerGroupBox);
110 rightLayout->addWidget(sensorsGroupBox);
113 void RobomonAtlantis::createPlaygroundGroupBox()
115 playgroundGroupBox = new QGroupBox(tr("Playground"));
116 QHBoxLayout *layout = new QHBoxLayout();
118 playgroundScene = new PlaygroundScene();
119 playgroundSceneView = new PlaygroundView(playgroundScene);
120 //playgroundSceneView->setMinimumWidth(630);
121 //playgroundSceneView->setMinimumHeight(445);
122 playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
123 playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
124 playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
125 layout->addWidget(playgroundSceneView);
127 playgroundGroupBox->setLayout(layout);
130 void RobomonAtlantis::createPositionGroupBox()
132 positionGroupBox = new QGroupBox(tr("Position state"));
133 positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
134 QGridLayout *layout = new QGridLayout();
136 actPosX = new QLineEdit();
137 actPosY = new QLineEdit();
138 actPosPhi = new QLineEdit();
140 estPosX = new QLineEdit();
141 estPosY = new QLineEdit();
142 estPosPhi = new QLineEdit();
144 actPosX->setReadOnly(true);
145 actPosY->setReadOnly(true);
146 actPosPhi->setReadOnly(true);
148 estPosX->setReadOnly(true);
149 estPosY->setReadOnly(true);
150 estPosPhi->setReadOnly(true);
152 layout->addWidget(MiscGui::createLabel("X"), 1, 0);
153 layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
154 layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
156 layout->addWidget(MiscGui::createLabel("X"), 5, 0);
157 layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
158 layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
160 layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
161 layout->addWidget(actPosX, 1, 1);
162 layout->addWidget(actPosY, 2, 1);
163 layout->addWidget(actPosPhi, 3, 1);
165 layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
166 layout->addWidget(estPosX, 5, 1);
167 layout->addWidget(estPosY, 6, 1);
168 layout->addWidget(estPosPhi, 7, 1);
170 positionGroupBox->setLayout(layout);
173 void RobomonAtlantis::createMiscGroupBox()
175 miscGroupBox = new QGroupBox(tr("Miscellaneous"));
176 miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
177 QGridLayout *layout = new QGridLayout();
179 showMapPushButton = new QCheckBox(tr("Show &map"));
180 showMapPushButton->setShortcut(tr("m"));
182 layout->addWidget(showMapPushButton, 0, 0);
184 obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
185 obstacleSimulationCheckBox->setShortcut(tr("o"));
186 layout->addWidget(obstacleSimulationCheckBox);
188 startPlug = new QCheckBox("Start plug");
189 layout->addWidget(startPlug);
191 puckInside = new QCheckBox("Puck inside");
192 layout->addWidget(puckInside);
194 miscGroupBox->setLayout(layout);
197 void RobomonAtlantis::createFSMGroupBox()
199 fsmGroupBox = new QGroupBox(tr("FSM"));
200 fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
201 QGridLayout *layout = new QGridLayout();
203 layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
204 fsm_main_state = new QLabel();
205 fsm_main_state->setMinimumWidth(100);
206 layout->addWidget(fsm_main_state, 1, 1);
208 layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
209 fsm_act_state = new QLabel();
210 layout->addWidget(fsm_act_state, 2, 1);
212 layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
213 fsm_motion_state = new QLabel();
214 layout->addWidget(fsm_motion_state, 3, 1);
216 fsmGroupBox->setLayout(layout);
219 void RobomonAtlantis::createDebugGroupBox()
221 debugGroupBox = new QGroupBox(tr("Debug window"));
222 debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
223 QHBoxLayout *layout = new QHBoxLayout();
225 debugWindow = new QTextEdit();
226 debugWindow->setReadOnly(true);
228 layout->addWidget(debugWindow);
229 debugGroupBox->setLayout(layout);
232 void RobomonAtlantis::createActuatorsGroupBox()
234 actuatorsGroupBox = new QGroupBox(tr("Actuators"));
235 actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
236 QHBoxLayout *layout = new QHBoxLayout();
238 createMotorsGroupBox();
239 createPickerGroupBox();
241 layout->setAlignment(Qt::AlignLeft);
242 layout->addWidget(enginesGroupBox);
243 layout->addWidget(pickerGroupBox);
244 actuatorsGroupBox->setLayout(layout);
247 void RobomonAtlantis::createDIOGroupBox()
249 dioGroupBox = new QGroupBox(tr("DIO"));
250 QGridLayout *layout = new QGridLayout();
253 font.setPointSize(5);
254 dioGroupBox->setFont(font);
256 for (int i=0; i<8; i++) {
257 diCheckBox[i] = new QCheckBox(QString("DI%1").arg(i));
258 doCheckBox[i] = new QCheckBox(QString("D0%1").arg(i));
259 layout->addWidget(diCheckBox[i], i, 0);
260 layout->addWidget(doCheckBox[i], i+8, 0);
263 dioGroupBox->setMaximumWidth(70);
264 dioGroupBox->setLayout(layout);
267 void RobomonAtlantis::createPowerGroupBox()
269 powerGroupBox = new QGroupBox(tr("Power management"));
270 powerGroupBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
271 powerGroupBox->setMaximumWidth(450);
272 QGridLayout *layout = new QGridLayout();
274 voltage33CheckBox = new QCheckBox(tr("&3.3V"));
275 voltage50CheckBox = new QCheckBox(tr("&5.0V"));
276 voltage80CheckBox = new QCheckBox(tr("&8.0V"));
278 voltage33CheckBox->setShortcut(tr("3"));
279 voltage50CheckBox->setShortcut(tr("5"));
280 voltage80CheckBox->setShortcut(tr("8"));
282 layout->addWidget(voltage33CheckBox, 0, 0);
283 layout->addWidget(voltage50CheckBox, 0, 2);
284 layout->addWidget(voltage80CheckBox, 0, 4); //1, 0);
285 layout->addWidget(MiscGui::createLabel("BAT"), 0, 6); //1, 2);
287 voltage33LineEdit = new QLineEdit();
288 voltage50LineEdit = new QLineEdit();
289 voltage80LineEdit = new QLineEdit();
290 voltageBATLineEdit = new QLineEdit();
292 voltage33LineEdit->setReadOnly(true);
293 voltage50LineEdit->setReadOnly(true);
294 voltage80LineEdit->setReadOnly(true);
295 voltageBATLineEdit->setReadOnly(true);
297 layout->addWidget(voltage33LineEdit, 0, 1);
298 layout->addWidget(voltage50LineEdit, 0, 3);
299 layout->addWidget(voltage80LineEdit, 0, 5); //1, 1);
300 layout->addWidget(voltageBATLineEdit, 0, 7); //1, 3);
302 powerGroupBox->setLayout(layout);
305 void RobomonAtlantis::createSensorsGroupBox()
307 sensorsGroupBox = new QGroupBox(tr("Sensors"));
308 sensorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
309 sensorsGroupBox->setMaximumWidth(450);
310 QHBoxLayout *layout = new QHBoxLayout();
312 layout->addWidget(MiscGui::createLabel("Puck Sharp"));
313 sharpPuck = new QProgressBar();
314 sharpPuck->setFormat("%v mm");
315 sharpPuck->setTextVisible(true);
316 sharpPuck->setMaximum(250);
317 layout->addWidget(sharpPuck);
319 layout->addWidget(MiscGui::createLabel("Lift"));
320 liftPos = new QProgressBar();
321 liftPos->setFormat("%v IRC");
322 liftPos->setTextVisible(true);
323 liftPos->setMaximum(1000);
324 layout->addWidget(liftPos);
326 layout->addWidget(MiscGui::createLabel("Pusher"));
327 pusherPos = new QProgressBar();
328 pusherPos->setFormat("%v IRC");
329 pusherPos->setTextVisible(true);
330 pusherPos->setMaximum(250);
331 layout->addWidget(pusherPos);
333 sensorsGroupBox->setLayout(layout);
336 void RobomonAtlantis::createMotorsGroupBox()
338 enginesGroupBox = new QGroupBox(tr("Motors"));
339 QVBoxLayout *layout = new QVBoxLayout();
340 QHBoxLayout *layout1 = new QHBoxLayout();
341 QHBoxLayout *layout2 = new QHBoxLayout();
343 leftMotorSlider = new QSlider(Qt::Vertical);
344 rightMotorSlider = new QSlider(Qt::Vertical);
345 bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
346 stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
348 leftMotorSlider->setMinimum(-100);
349 leftMotorSlider->setMaximum(100);
350 leftMotorSlider->setTracking(false);
351 leftMotorSlider->setTickPosition(QSlider::TicksLeft);
353 rightMotorSlider->setMinimum(-100);
354 rightMotorSlider->setMaximum(100);
355 rightMotorSlider->setTracking(false);
356 rightMotorSlider->setTickPosition(QSlider::TicksRight);
358 stopMotorsPushButton->setMaximumWidth(90);
360 layout1->addWidget(leftMotorSlider);
361 layout1->addWidget(MiscGui::createLabel("0"));
362 layout1->addWidget(rightMotorSlider);
364 layout2->addWidget(bothMotorsCheckBox);
366 layout->addWidget(MiscGui::createLabel("100"));
367 layout->addLayout(layout1);
368 layout->addWidget(MiscGui::createLabel("-100"));
369 layout->addLayout(layout2);
370 layout->addWidget(stopMotorsPushButton);
371 enginesGroupBox->setLayout(layout);
374 void RobomonAtlantis::createPickerGroupBox()
376 pickerGroupBox = new QGroupBox(tr("Picker"));
377 QVBoxLayout *layout = new QVBoxLayout();
379 leftBeltCheckBox = new QCheckBox(tr("L Belt"));
380 rightBeltCheckBox = new QCheckBox(tr("R Belt"));
381 leftChelaCheckBox = new QCheckBox(tr("L Chela"));
382 rightChelaCheckBox = new QCheckBox(tr("R Chela"));
383 leftBeltDial = new QDial();
384 rightBeltDial = new QDial();
385 leftChelaDial = new QDial();
386 rightChelaDial = new QDial();
387 pickPushButton = new QPushButton(tr("Pick!"));
389 leftBeltDial->setMinimum(0);
390 leftBeltDial->setMaximum(200);
391 leftBeltDial->setValue(100);
393 rightBeltDial->setMinimum(0);
394 rightBeltDial->setMaximum(200);
395 rightBeltDial->setValue(100);
397 leftChelaDial->setMinimum(0);
398 leftChelaDial->setMaximum(255);
399 leftChelaDial->setValue(127);
401 rightChelaDial->setMinimum(0);
402 rightChelaDial->setMaximum(255);
403 rightChelaDial->setValue(127);
405 layout->addWidget(leftChelaDial);
406 layout->addWidget(leftChelaCheckBox);
407 layout->addWidget(rightChelaDial);
408 layout->addWidget(rightChelaCheckBox);
409 layout->addWidget(leftBeltDial);
410 layout->addWidget(leftBeltCheckBox);
411 layout->addWidget(rightBeltDial);
412 layout->addWidget(rightBeltCheckBox);
413 layout->addWidget(pickPushButton);
415 pickerGroupBox->setLayout(layout);
418 void RobomonAtlantis::createRobots()
420 robotActPos = new Robot(QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
421 robotActPos->setZValue(11);
422 robotEstPos = new Robot(QPen(), QBrush(Qt::darkGray));
423 robotEstPos->setZValue(10);
425 playgroundScene->addItem(robotActPos);
426 playgroundScene->addItem(robotEstPos);
429 /**********************************************************************
431 **********************************************************************/
432 void RobomonAtlantis::createActions()
434 /* power management */
435 connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
436 this, SLOT(setVoltage33(int)));
437 connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
438 this, SLOT(setVoltage50(int)));
439 connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
440 this, SLOT(setVoltage80(int)));
443 connect(leftMotorSlider, SIGNAL(valueChanged(int)),
444 this, SLOT(setLeftMotor(int)));
445 connect(rightMotorSlider, SIGNAL(valueChanged(int)),
446 this, SLOT(setRightMotor(int)));
447 connect(stopMotorsPushButton, SIGNAL(clicked()),
448 this, SLOT(stopMotors()));
451 for (int i=0; i<8; i++)
452 connect(doCheckBox[0], SIGNAL(stateChanged(int)),
453 this, SLOT(setDO(int)));
455 /* path planning map */
456 connect(showMapPushButton, SIGNAL(clicked()),
457 this, SLOT(showMap()));
459 connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
460 connect(puckInside, SIGNAL(stateChanged(int)), this, SLOT(sendPuckInside(int)));
462 /* obstacle simulation */
463 simulationEnabled = 0;
464 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
465 this, SLOT(setSimulation(int)));
466 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
467 this, SLOT(setObstacleSimulation(int)));
468 connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
469 playgroundScene, SLOT(showObstacle(int)));
470 connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
471 this, SLOT(changeObstacle(QPointF)));
474 connect(pickPushButton, SIGNAL(clicked()), this, SLOT(pick()));
475 connect(leftBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
476 connect(rightBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
477 connect(leftChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
478 connect(rightChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
479 connect(leftBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
480 connect(rightBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
481 connect(leftChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
482 connect(rightChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
485 void RobomonAtlantis::setVoltage33(int state)
488 orte.pwr_ctrl.voltage33 = true;
490 orte.pwr_ctrl.voltage33 = false;
493 void RobomonAtlantis::setVoltage50(int state)
496 orte.pwr_ctrl.voltage50 = true;
498 orte.pwr_ctrl.voltage50 = false;
501 void RobomonAtlantis::setVoltage80(int state)
504 orte.pwr_ctrl.voltage80 = true;
506 orte.pwr_ctrl.voltage80 = false;
509 void RobomonAtlantis::setLeftMotor(int value)
512 short int rightMotor;
514 if(bothMotorsCheckBox->isChecked())
515 rightMotorSlider->setValue(value);
517 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
518 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
520 orte.motion_speed.left = leftMotor;
521 orte.motion_speed.right = rightMotor;
525 void RobomonAtlantis::setRightMotor(int value)
528 short int rightMotor;
530 if(bothMotorsCheckBox->isChecked())
531 leftMotorSlider->setValue(value);
533 leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
534 rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
536 orte.motion_speed.left = leftMotor;
537 orte.motion_speed.right = rightMotor;
541 void RobomonAtlantis::stopMotors()
543 leftMotorSlider->setValue(0);
544 rightMotorSlider->setValue(0);
548 void RobomonAtlantis::setDO(int state)
551 /* FIXME: digital output control comes here */
554 void RobomonAtlantis::pick()
556 // TODO: send signal to the fsmact
559 void RobomonAtlantis::setBelts(int value)
562 unsigned char leftBelt;
563 unsigned char rightBelt;
565 leftBelt = (unsigned char)leftBeltDial->value();
566 rightBelt = (unsigned char)rightBeltDial->value();
568 act_belts(leftBelt, rightBelt);
571 void RobomonAtlantis::setChelae(int value)
574 unsigned char leftChela;
575 unsigned char rightChela;
577 leftChela = (unsigned char)leftChelaDial->value();
578 rightChela = (unsigned char)rightChelaDial->value();
580 act_chelae(leftChela, rightChela);
583 void RobomonAtlantis::showMap()
587 if (sharedMemoryOpened == false)
590 mapTimer = new QTimer(this);
591 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
592 mapTimer->start(200);
594 disconnect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
595 connect(showMapPushButton, SIGNAL(clicked()),
596 this, SLOT(showPlayground()));
597 playgroundScene->showMap(true);
600 void RobomonAtlantis::showPlayground()
603 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
605 disconnect(showMapPushButton, SIGNAL(clicked()),
606 this, SLOT(showPlayground()));
607 connect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
608 playgroundScene->showMap(false);
611 void RobomonAtlantis::paintMap()
614 struct map *map = ShmapIsMapInit();
618 for(int i=0; i < MAP_WIDTH; i++) {
619 for(int j=0; j<MAP_HEIGHT; j++) {
622 struct map_cell *cell = &map->cells[j][i];
625 if ((cell->flags & MAP_FLAG_WALL) &&
626 (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
628 if (cell->flags & MAP_FLAG_IGNORE_OBST)
630 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
632 if (cell->flags & MAP_FLAG_PATH)
634 if (cell->flags & MAP_FLAG_START)
636 if (cell->flags & MAP_FLAG_GOAL)
638 if (cell->detected_obstacle) {
639 QColor c1(color), c2(blue);
640 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
641 QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
642 c1.green() + (int)(f*(c2.green() - c1.green())),
643 c1.blue() + (int)(f*(c2.blue() - c1.blue())));
646 if (cell->flags & MAP_FLAG_DET_OBST)
649 playgroundScene->setMapColor(i, j, color);
654 void RobomonAtlantis::setSimulation(int state)
657 robottype_publisher_hokuyo_scan_create(&orte,
658 dummy_publisher_callback, this);
660 if (!simulationEnabled)
662 robottype_publisher_hokuyo_scan_destroy(&orte);
664 simulationEnabled = state;
668 \fn RobomonAtlantis::setObstacleSimulation(int state)
670 void RobomonAtlantis::setObstacleSimulation(int state)
673 /* TODO Maybe it is possible to attach only once to Shmap */
675 obstacleSimulationTimer = new QTimer(this);
676 connect(obstacleSimulationTimer, SIGNAL(timeout()),
677 this, SLOT(simulateObstaclesHokuyo()));
678 obstacleSimulationTimer->start(100);
679 setMouseTracking(true);
681 if (obstacleSimulationTimer)
682 delete obstacleSimulationTimer;
683 //double distance = 0.8;
688 void RobomonAtlantis::simulateObstaclesHokuyo()
690 double distance, wall_distance;
692 uint16_t *hokuyo = orte.hokuyo_scan.data;
694 for (i=0; i<HOKUYO_CLUSTER_CNT; i++) {
695 wall_distance = distanceToWallHokuyo(i);
696 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
697 if (wall_distance < distance)
698 distance = wall_distance;
699 hokuyo[i] = distance*1000;
701 ORTEPublicationSend(orte.publication_hokuyo_scan);
705 void RobomonAtlantis::changeObstacle(QPointF position)
707 if (!simulationEnabled) {
708 simulationEnabled = 1;
709 obstacleSimulationCheckBox->setChecked(true);
712 simulatedObstacle.x = position.x();
713 simulatedObstacle.y = position.y();
714 simulateObstaclesHokuyo();
717 /**********************************************************************
719 **********************************************************************/
720 bool RobomonAtlantis::event(QEvent *event)
722 switch (event->type()) {
723 case QEVENT(QEV_MOTION_STATUS):
724 emit motionStatusReceivedSignal();
726 case QEVENT(QEV_ACTUAL_POSITION):
727 emit actualPositionReceivedSignal();
729 case QEVENT(QEV_ESTIMATED_POSITION):
730 emit estimatedPositionReceivedSignal();
733 emit diReceivedSignal();
735 case QEVENT(QEV_ACCELEROMETER):
736 emit accelerometerReceivedSignal();
738 case QEVENT(QEV_ACCUMULATOR):
739 emit accumulatorReceivedSignal();
741 case QEVENT(QEV_POWER_VOLTAGE):
742 emit powerVoltageReceivedSignal();
744 case QEVENT(QEV_FSM_MAIN):
745 fsm_main_state->setText(orte.fsm_main.state_name);
747 case QEVENT(QEV_FSM_ACT):
748 fsm_act_state->setText(orte.fsm_act.state_name);
750 case QEVENT(QEV_FSM_MOTION):
751 fsm_motion_state->setText(orte.fsm_motion.state_name);
753 case QEVENT(QEV_PUCK_DISTANCE):
754 sharpPuck->setValue(orte.puck_distance.distance*1000);
756 case QEVENT(QEV_LIFT_PUSHER):
757 liftPos->setValue(orte.actuator_status.lift_pos);
758 pusherPos->setValue(orte.actuator_status.pusher_pos);
761 if (event->type() == QEvent::Close)
762 closeEvent((QCloseEvent *)event);
763 else if (event->type() == QEvent::KeyPress)
764 keyPressEvent((QKeyEvent *)event);
765 else if (event->type() == QEvent::KeyRelease)
766 keyReleaseEvent((QKeyEvent *)event);
767 else if (event->type() == QEvent::FocusIn)
769 else if (event->type() == QEvent::FocusOut)
781 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
785 if (event->isAutoRepeat()) {
786 switch (event->key()) {
788 peak = leftMotorSlider->minimum()/2;
789 if (leftMotorValue < peak ||
790 rightMotorValue < peak)
794 leftMotorValue *= gain;
795 rightMotorValue *= gain;
796 leftMotorSlider->setValue((int)leftMotorValue);
797 rightMotorSlider->setValue((int)rightMotorValue);
803 peak = leftMotorSlider->maximum()/2;
804 if (leftMotorValue > peak ||
805 rightMotorValue > peak)
809 leftMotorValue *= gain;
810 rightMotorValue *= gain;
811 leftMotorSlider->setValue((int)leftMotorValue);
812 rightMotorSlider->setValue((int)rightMotorValue);
822 switch (event->key()) {
826 bothMotorsCheckBox->setChecked(true);
827 leftMotorSlider->setValue((int)leftMotorValue);
828 setLeftMotor((int)leftMotorValue);
832 rightMotorValue = -1;
833 bothMotorsCheckBox->setChecked(true);
834 leftMotorSlider->setValue((int)leftMotorValue);
835 setLeftMotor((int)leftMotorValue);
840 leftMotorSlider->setValue((int)leftMotorValue);
841 rightMotorSlider->setValue((int)rightMotorValue);
842 setLeftMotor((int)leftMotorValue);
843 setRightMotor((int)leftMotorValue);
847 rightMotorValue = -1;
848 leftMotorSlider->setValue((int)leftMotorValue);
849 rightMotorSlider->setValue((int)rightMotorValue);
850 setLeftMotor((int)leftMotorValue);
851 setRightMotor((int)rightMotorValue);
860 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
862 if (event->isAutoRepeat()) {
867 switch (event->key()) {
874 bothMotorsCheckBox->setChecked(false);
875 leftMotorSlider->setValue((int)leftMotorValue);
876 rightMotorSlider->setValue((int)rightMotorValue);
885 void RobomonAtlantis::closeEvent(QCloseEvent *)
887 robottype_roboorte_destroy(&orte);
890 /**********************************************************************
892 **********************************************************************/
893 void RobomonAtlantis::createOrte()
899 rv = robottype_roboorte_init(&orte);
901 printf("RobomonAtlantis: Unable to initialize ORTE\n");
905 robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
907 robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
908 robottype_publisher_lift_create(&orte, NULL, &orte);
909 robottype_publisher_pusher_create(&orte, NULL, &orte);
910 robottype_publisher_chelae_create(&orte, NULL, &orte);
911 robottype_publisher_belts_create(&orte, NULL, &orte);
912 robottype_publisher_holder_create(&orte, NULL, &orte);
913 robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
914 robottype_publisher_puck_inside_create(&orte, NULL, &orte);
917 robottype_subscriber_pwr_voltage_create(&orte,
918 receivePowerVoltageCallBack, this);
919 robottype_subscriber_motion_status_create(&orte,
920 receiveMotionStatusCallBack, this);
921 robottype_subscriber_ref_pos_create(&orte,
922 receiveActualPositionCallBack, this);
923 robottype_subscriber_est_pos_create(&orte,
924 receiveEstimatedPositionCallBack, this);
925 robottype_subscriber_fsm_main_create(&orte,
926 rcv_fsm_main_cb, this);
927 robottype_subscriber_fsm_motion_create(&orte,
928 rcv_fsm_motion_cb, this);
929 robottype_subscriber_fsm_act_create(&orte,
930 rcv_fsm_act_cb, this);
931 robottype_subscriber_puck_distance_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_PUCK_DISTANCE));
932 robottype_subscriber_actuator_status_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_LIFT_PUSHER));
935 /*createDISubscriber(this, &orteData);*/
936 /*createAccelerometerSubscriber(this, &orteData);*/
937 /*createAccumulatorSubscriber(this, &orteData);*/
939 orte.motion_speed.left = 0;
940 orte.motion_speed.right = 0;
941 /* power management */
942 orte.pwr_ctrl.voltage33 = true;
943 orte.pwr_ctrl.voltage50 = true;
944 orte.pwr_ctrl.voltage80 = true;
945 voltage33CheckBox->setChecked(true);
946 voltage50CheckBox->setChecked(true);
947 voltage80CheckBox->setChecked(true);
951 /* set actions to do when we receive data from orte */
952 connect(this, SIGNAL(motionStatusReceivedSignal()),
953 this, SLOT(motionStatusReceived()));
954 connect(this, SIGNAL(actualPositionReceivedSignal()),
955 this, SLOT(actualPositionReceived()));
956 connect(this, SIGNAL(estimatedPositionReceivedSignal()),
957 this, SLOT(estimatedPositionReceived()));
958 connect(this, SIGNAL(diReceivedSignal()),
959 this, SLOT(diReceived()));
960 connect(this, SIGNAL(accelerometerReceivedSignal()),
961 this, SLOT(accelerometerReceived()));
962 connect(this, SIGNAL(accumulatorReceivedSignal()),
963 this, SLOT(accumulatorReceived()));
964 connect(this, SIGNAL(powerVoltageReceivedSignal()),
965 this, SLOT(powerVoltageReceived()));
968 void RobomonAtlantis::motionStatusReceived()
970 WDBG("ORTE received: motion status");
973 void RobomonAtlantis::actualPositionReceived()
975 actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
976 actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
977 actPosPhi->setText(QString("%1(%2)")
978 .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
979 .arg(orte.ref_pos.phi, 0, 'f', 1));
980 robotActPos->moveRobot(orte.ref_pos.x,
981 orte.ref_pos.y, orte.ref_pos.phi);
984 void RobomonAtlantis::estimatedPositionReceived()
986 estPosX->setText(QString("%1").arg(orte.est_pos.x, 0, 'f', 3));
987 estPosY->setText(QString("%1").arg(orte.est_pos.y, 0, 'f', 3));
988 estPosPhi->setText(QString("%1(%2)")
989 .arg(DEGREES(orte.est_pos.phi), 0, 'f', 0)
990 .arg(orte.est_pos.phi, 0, 'f', 1));
991 robotEstPos->moveRobot(orte.est_pos.x,
992 orte.est_pos.y, orte.est_pos.phi);
995 void RobomonAtlantis::diReceived()
997 WDBG("ORTE received: DI");
1000 void RobomonAtlantis::accelerometerReceived()
1002 WDBG("ORTE received: accelerometer");
1005 void RobomonAtlantis::accumulatorReceived()
1007 WDBG("ORTE received: accumulator");
1010 void RobomonAtlantis::powerVoltageReceived()
1012 voltage33LineEdit->setText(QString("%1").arg(
1013 orte.pwr_voltage.voltage33, 0, 'f', 3));
1014 voltage50LineEdit->setText(QString("%1").arg(
1015 orte.pwr_voltage.voltage50, 0, 'f', 3));
1016 voltage80LineEdit->setText(QString("%1").arg(
1017 orte.pwr_voltage.voltage80, 0, 'f', 3));
1018 voltageBATLineEdit->setText(QString("%1").arg(
1019 orte.pwr_voltage.voltageBAT, 0, 'f', 3));
1023 /**********************************************************************
1025 **********************************************************************/
1026 void RobomonAtlantis::openSharedMemory()
1029 int sharedSegmentSize;
1031 if (sharedMemoryOpened)
1034 sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
1036 /* Get segment identificator in a read only mode */
1037 segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
1038 if(segmentId == -1) {
1039 QMessageBox::critical(this, "robomon",
1040 "Unable to open shared memory segment!");
1047 /* Attach the shared memory segment */
1048 //map = (_Map*)shmat (segmentId, (void*) 0, 0);
1050 sharedMemoryOpened = true;
1053 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
1055 double distance=4.0, min_distance=4.0;
1058 struct map *map = ShmapIsMapInit();
1060 if (!map) return min_distance;
1062 // Simulate obstacles
1063 for(j=0;j<MAP_HEIGHT;j++) {
1064 for (i=0;i<MAP_WIDTH;i++) {
1065 struct map_cell *cell = &map->cells[j][i];
1066 if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
1068 ShmapCell2Point(i, j, &wall.x, &wall.y);
1070 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
1071 if (distance<min_distance) min_distance = distance;
1076 return min_distance;
1080 * Calculation for Hokuyo simulation. Calculates distance that would
1081 * be returned by Hokuyo sensors, if there is only one obstacle (as
1082 * specified by parameters).
1084 * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
1085 * @param obstacle Position of the obstacle (x, y in meters).
1086 * @param obstacleSize Size (diameter) of the obstacle in meters.
1088 * @return Distance measured by sensors in meters.
1090 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
1092 struct est_pos_type e = orte.est_pos;
1096 s.x = HOKUYO_CENTER_OFFSET_M;
1098 s.ang = HOKUYO_CLUSTER_TO_RAD(beamnum);
1100 Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
1101 e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
1102 sensor_a = e.phi + s.ang;
1104 const double sensorRange = 4.0; /*[meters]*/
1106 double distance, angle;
1108 angle = sensor.angleTo(obstacle) - sensor_a;
1109 angle = fmod(angle, 2.0*M_PI);
1110 if (angle > +M_PI) angle -= 2.0*M_PI;
1111 if (angle < -M_PI) angle += 2.0*M_PI;
1112 angle = fabs(angle);
1113 distance = sensor.distanceTo(obstacle)-0.11;
1114 if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
1115 // We can see the obstackle from here.
1116 if (angle < M_PI/2.0) {
1117 distance = distance/cos(angle);
1119 if (distance > sensorRange)
1120 distance = sensorRange;
1122 distance = sensorRange;
1128 void RobomonAtlantis::sendStart(int plug)
1130 orte.robot_cmd.start = plug ? 0 : 1;
1131 ORTEPublicationSend(orte.publication_robot_cmd);
1134 void RobomonAtlantis::sendPuckInside(int value)
1136 orte.puck_inside.status = value ? 1 : 0;
1137 ORTEPublicationSend(orte.publication_puck_inside);