src/robomon/RobomonAtlantis.cpp

   1 /*
   2  * RobomonAtlantis.cpp                  07/10/31
   3  *
   4  * Robot`s visualization and control GUI for robot of the
   5  * Eurobot 2008 (Mission to Mars).
   6  *
   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
  11  */
  12
  13 #include <QtGui>
  14 #include <queue>
  15 #include <cstdlib>
  16 #include <sys/shm.h>
  17 #include <sys/stat.h>
  18 #include <sys/socket.h>
  19 #include <netinet/in.h>
  20 #include <arpa/inet.h>
  21 #include <stdio.h>
  22 #include <unistd.h>
  23 #include <math.h>
  24 #include <errno.h>
  25
  26 #include <orte.h>
  27 #include <path_planner.h>
  28 #include <robodim.h>
  29 #include <sharp.h>
  30 #include <trgen.h>
  31 #include <map.h>
  32 #include <robomath.h>
  33 #include <lidar_params.h>
  34 #include <actuators.h>
  35 #include "PlaygroundScene.h"
  36 #include "MiscGui.h"
  37 #include "robomon_orte.h"
  38 #include "RobomonAtlantis.h"
  39 #include "playgroundview.h"
  40 #include "trail.h"
  41
  42 #include <QCoreApplication>
  43 #include <QEvent>
  44 #include <QKeyEvent>
  45 #include <QDebug>
  46 #include <QMessageBox>
  47
  48 RobomonAtlantis::RobomonAtlantis(QStatusBar *_statusBar)
  49         : QWidget(0), statusBar(_statusBar)
  50 {
  51         QFont font;
  52         font.setPointSize(7);
  53         setFont(font);
  54
  55         debugWindowEnabled = false;
  56
  57         createLeftLayout();
  58         createRightLayout();
  59
  60         QHBoxLayout *mainLayout = new QHBoxLayout;
  61         mainLayout->addLayout(leftLayout);
  62         mainLayout->addLayout(rightLayout);
  63         setLayout(mainLayout);
  64
  65         createOrte();
  66         createRobots();
  67         createActions();
  68         createMap();
  69
  70         setFocusPolicy(Qt::StrongFocus);
  71         sharedMemoryOpened = false;
  72         WDBG("Youuuhouuuu!!");
  73 }
  74
  75 /**********************************************************************
  76  * GUI
  77  **********************************************************************/
  78 void RobomonAtlantis::createLeftLayout()
  79 {
  80         leftLayout = new QVBoxLayout();
  81
  82         createDebugGroupBox();
  83         debugWindowEnabled = true;
  84         createPlaygroundGroupBox();
  85         leftLayout->addWidget(playgroundGroupBox);
  86         //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab
  87 }
  88
  89 void RobomonAtlantis::createRightLayout()
  90 {
  91         rightLayout = new QVBoxLayout();
  92
  93         createPositionGroupBox();
  94         createObstSimGroupBox();
  95         createMiscGroupBox();
  96         createFSMGroupBox();
  97         createActuatorsGroupBox();
  98         createPowerGroupBox();
  99
 100         rightLayout->addWidget(positionGroupBox);
 101         rightLayout->addWidget(obstSimGroupBox);
 102         rightLayout->addWidget(miscGroupBox);
 103         rightLayout->addWidget(fsmGroupBox);
 104         rightLayout->addWidget(actuatorsGroupBox);
 105 }
 106
 107 void RobomonAtlantis::createPlaygroundGroupBox()
 108 {
 109         playgroundGroupBox = new QGroupBox(tr("Playground"));
 110         QHBoxLayout *layout = new QHBoxLayout();
 111
 112         playgroundScene = new PlaygroundScene();
 113         playgroundSceneView = new PlaygroundView(playgroundScene);
 114         playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
 115         playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
 116         playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 117         playgroundSceneView->setMouseTracking(true);
 118         layout->addWidget(playgroundSceneView);
 119
 120         playgroundGroupBox->setLayout(layout);
 121 }
 122
 123 void RobomonAtlantis::createPositionGroupBox()
 124 {
 125         positionGroupBox = new QGroupBox(tr("Position state"));
 126         positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 127         QGridLayout *layout = new QGridLayout();
 128
 129         actPosX = new QLineEdit();
 130         actPosY = new QLineEdit();
 131         actPosPhi = new QLineEdit();
 132
 133         estPosX = new QLineEdit();
 134         estPosY = new QLineEdit();
 135         estPosPhi = new QLineEdit();
 136
 137         actPosX->setReadOnly(true);
 138         actPosY->setReadOnly(true);
 139         actPosPhi->setReadOnly(true);
 140
 141         estPosX->setReadOnly(true);
 142         estPosY->setReadOnly(true);
 143         estPosPhi->setReadOnly(true);
 144
 145         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 146         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 147         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 148
 149         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 150         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 151         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 152
 153         layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
 154         layout->addWidget(actPosX, 1, 1);
 155         layout->addWidget(actPosY, 2, 1);
 156         layout->addWidget(actPosPhi, 3, 1);
 157
 158         layout->addWidget(MiscGui::createLabel("Estimated (indep. odo.)", Qt::AlignLeft), 4, 1);
 159         layout->addWidget(estPosX, 5, 1);
 160         layout->addWidget(estPosY, 6, 1);
 161         layout->addWidget(estPosPhi, 7, 1);
 162
 163         positionGroupBox->setLayout(layout);
 164 }
 165
 166 void RobomonAtlantis::createMiscGroupBox()
 167 {
 168         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 169         miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 170         QGridLayout *layout = new QGridLayout();
 171
 172         startPlug = new QCheckBox("&Start plug");
 173         layout->addWidget(startPlug);
 174
 175         colorChoser = new QCheckBox("&Team color");
 176         layout->addWidget(colorChoser);
 177
 178         strategyButton= new QPushButton(tr("Strategy"));
 179         layout->addWidget(strategyButton);
 180
 181         miscGroupBox->setLayout(layout);
 182 }
 183
 184 void RobomonAtlantis::createObstSimGroupBox()
 185 {
 186         obstSimGroupBox = new QGroupBox(tr("Obstacle simulation"));
 187         obstSimGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 188         QGridLayout *layout = new QGridLayout();
 189
 190         hokuyoSimCheckBox = new QCheckBox(tr("&Hokuyo lidar simulation"));
 191         hokuyoSimCheckBox->setShortcut(tr("h"));
 192         layout->addWidget(hokuyoSimCheckBox);
 193
 194         sick331SimCheckBox = new QCheckBox(tr("Sick Tim &331 lidar simulation"));
 195         sick331SimCheckBox->setShortcut(tr("3"));
 196         layout->addWidget(sick331SimCheckBox);
 197
 198         sick551SimCheckBox = new QCheckBox(tr("Sick Tim &551 lidar simulation"));
 199         sick551SimCheckBox->setShortcut(tr("5"));
 200         layout->addWidget(sick551SimCheckBox);
 201
 202         obstSimGroupBox->setLayout(layout);
 203 }
 204
 205 void RobomonAtlantis::createFSMGroupBox()
 206 {
 207         fsmGroupBox = new QGroupBox(tr("FSM"));
 208         fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 209         QGridLayout *layout = new QGridLayout();
 210
 211         layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
 212         fsm_main_state = new QLabel();
 213         fsm_main_state->setMinimumWidth(100);
 214         fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 215         layout->addWidget(fsm_main_state, 1, 1);
 216
 217         layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
 218         fsm_act_state = new QLabel();
 219         layout->addWidget(fsm_act_state, 2, 1);
 220
 221         layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
 222         fsm_motion_state = new QLabel();
 223         layout->addWidget(fsm_motion_state, 3, 1);
 224
 225         fsmGroupBox->setLayout(layout);
 226 }
 227
 228 void RobomonAtlantis::createDebugGroupBox()
 229 {
 230         debugGroupBox = new QGroupBox(tr("Debug window"));
 231         debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
 232         QHBoxLayout *layout = new QHBoxLayout();
 233
 234         debugWindow = new QTextEdit();
 235         debugWindow->setReadOnly(true);
 236
 237         layout->addWidget(debugWindow);
 238         debugGroupBox->setLayout(layout);
 239 }
 240
 241 void RobomonAtlantis::createActuatorsGroupBox()
 242 {
 243         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 244         actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
 245         QHBoxLayout *layout = new QHBoxLayout();
 246
 247         layout->setAlignment(Qt::AlignLeft);
 248         actuatorsGroupBox->setLayout(layout);
 249 }
 250
 251 void RobomonAtlantis::createPowerGroupBox()
 252 {
 253         powerGroupBox = new QGroupBox(tr("Power management"));
 254         powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 255         QGridLayout *layout = new QGridLayout();
 256
 257         voltage33CheckBox = new QCheckBox(tr("&3.3V"));
 258         voltage50CheckBox = new QCheckBox(tr("&5.0V"));
 259         voltage80CheckBox = new QCheckBox(tr("&8.0V"));
 260
 261         voltage33CheckBox->setShortcut(tr("3"));
 262         voltage50CheckBox->setShortcut(tr("5"));
 263         voltage80CheckBox->setShortcut(tr("8"));
 264
 265         layout->addWidget(voltage33CheckBox, 0, 0);
 266         layout->addWidget(voltage50CheckBox, 1, 0);
 267         layout->addWidget(voltage80CheckBox, 2, 0);
 268         layout->addWidget(MiscGui::createLabel("BAT"), 3, 0);
 269
 270         voltage33LineEdit = new QLineEdit();
 271         voltage50LineEdit = new QLineEdit();
 272         voltage80LineEdit = new QLineEdit();
 273         voltageBATLineEdit = new QLineEdit();
 274
 275         voltage33LineEdit->setReadOnly(true);
 276         voltage50LineEdit->setReadOnly(true);
 277         voltage80LineEdit->setReadOnly(true);
 278         voltageBATLineEdit->setReadOnly(true);
 279
 280         layout->addWidget(voltage33LineEdit, 0, 1);
 281         layout->addWidget(voltage50LineEdit, 1, 1);
 282         layout->addWidget(voltage80LineEdit, 2, 1);
 283         layout->addWidget(voltageBATLineEdit, 3, 1);
 284
 285         powerGroupBox->setLayout(layout);
 286 }
 287
 288 void RobomonAtlantis::createRobots()
 289 {
 290         robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
 291         robotRefPos->setZValue(11);
 292         trailRefPos = new Trail(QPen(Qt::darkBlue));
 293         trailRefPos->setZValue(11);
 294
 295         robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray));
 296         robotEstPosBest->setZValue(10);
 297         trailEstPosBest = new Trail(QPen());
 298         trailEstPosBest->setZValue(10);
 299
 300         robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
 301         robotEstPosOdo->setZValue(10);
 302         trailOdoPos = new Trail(QPen(Qt::red));
 303         trailOdoPos->setZValue(10);
 304
 305         robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen));
 306         robotEstPosIndepOdo->setZValue(10);
 307         trailPosIndepOdo = new Trail(QPen(Qt::green));
 308         trailPosIndepOdo->setZValue(10);
 309
 310         playgroundScene->addItem(robotRefPos);
 311         playgroundScene->addItem(robotEstPosBest);
 312         playgroundScene->addItem(robotEstPosIndepOdo);
 313         playgroundScene->addItem(robotEstPosOdo);
 314
 315         showTrails(false);
 316
 317         playgroundScene->addItem(trailRefPos);
 318         playgroundScene->addItem(trailPosIndepOdo);
 319         playgroundScene->addItem(trailOdoPos);
 320
 321         hokuyoScan = new LidarScan(hokuyo_params);
 322         hokuyoScan->setZValue(10);
 323         playgroundScene->addItem(hokuyoScan);
 324
 325         sickScan = new LidarScan(sick_params);
 326         sickScan->setZValue(10);
 327         playgroundScene->addItem(sickScan);
 328
 329         sick551Scan = new LidarScan(sick551_params);
 330         sick551Scan->setZValue(10);
 331         playgroundScene->addItem(sick551Scan);
 332 }
 333
 334 void RobomonAtlantis::createMap()
 335 {
 336         mapImage = new Map();
 337         mapImage->setZValue(5);
 338         mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
 339
 340         playgroundScene->addItem(mapImage);
 341 }
 342
 343 /**********************************************************************
 344  * GUI actions
 345  **********************************************************************/
 346 void RobomonAtlantis::createActions()
 347 {
 348         /* power management */
 349         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 350                         this, SLOT(setVoltage33(int)));
 351         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 352                         this, SLOT(setVoltage50(int)));
 353         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 354                         this, SLOT(setVoltage80(int)));
 355
 356         connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
 357         connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int)));
 358         connect(strategyButton, SIGNAL(pressed()), this, SLOT(changeStrategy_1()));
 359         connect(strategyButton, SIGNAL(released()), this, SLOT(changeStrategy_0()));
 360
 361         /* obstacle simulation */
 362         hokuyoSimEnabled = 0;
 363         sickSimEnabled = 0;
 364         sick551SimEnabled = 0;
 365
 366         connect(hokuyoSimCheckBox, SIGNAL(stateChanged(int)),
 367                         this, SLOT(setHokuyoSimulation(int)));
 368         connect(hokuyoSimCheckBox, SIGNAL(stateChanged(int)),
 369                         this, SLOT(setHokuyoObstacleSimulation(int)));
 370
 371         connect(sick331SimCheckBox, SIGNAL(stateChanged(int)),
 372                         this, SLOT(setSick331Simulation(int)));
 373         connect(sick331SimCheckBox, SIGNAL(stateChanged(int)),
 374                         this, SLOT(setSick331ObstacleSimulation(int)));
 375
 376         connect(sick551SimCheckBox, SIGNAL(stateChanged(int)),
 377                         this, SLOT(setSick551Simulation(int)));
 378         connect(sick551SimCheckBox, SIGNAL(stateChanged(int)),
 379                         this, SLOT(setSick551ObstacleSimulation(int)));
 380
 381         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 382                         this, SLOT(changeObstacle(QPointF)));
 383 }
 384
 385 void RobomonAtlantis::changeStrategy_1()
 386 {
 387                 orte.robot_switches.strategy = true;
 388                 ORTEPublicationSend(orte.publication_robot_switches);
 389 }
 390
 391 void RobomonAtlantis::changeStrategy_0()
 392 {
 393                 orte.robot_switches.strategy = false;
 394                 ORTEPublicationSend(orte.publication_robot_switches);
 395 }
 396
 397 void RobomonAtlantis::setVoltage33(int state)
 398 {
 399         if (state)
 400                 orte.pwr_ctrl.voltage33 = true;
 401         else
 402                 orte.pwr_ctrl.voltage33 = false;
 403 }
 404
 405 void RobomonAtlantis::setVoltage50(int state)
 406 {
 407         if (state)
 408                 orte.pwr_ctrl.voltage50 = true;
 409         else
 410                 orte.pwr_ctrl.voltage50 = false;
 411 }
 412
 413 void RobomonAtlantis::setVoltage80(int state)
 414 {
 415         if (state)
 416                 orte.pwr_ctrl.voltage80 = true;
 417         else
 418                 orte.pwr_ctrl.voltage80 = false;
 419 }
 420
 421 void RobomonAtlantis::useOpenGL(bool use)
 422 {
 423         playgroundSceneView->useOpenGL(&use);
 424 }
 425
 426 void RobomonAtlantis::showMap(bool show)
 427 {
 428         openSharedMemory();
 429
 430         if (sharedMemoryOpened == false)
 431                 return;
 432
 433         if (show) {
 434                 mapTimer = new QTimer(this);
 435                 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 436                 mapTimer->start(200);
 437         } else {
 438                 if(mapTimer != NULL) {
 439                         mapTimer->stop();
 440                         disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 441                 }
 442         }
 443         mapImage->setVisible(show);
 444 }
 445
 446 void RobomonAtlantis::paintMap()
 447 {
 448                 using namespace Qt;
 449                 struct map *map = ShmapIsMapInit();
 450
 451                 if (!map) return;
 452
 453         for(int i = 0; i < MAP_WIDTH; i++) {
 454                 for(int j = 0; j < MAP_HEIGHT; j++) {
 455                                                 QColor color;
 456
 457                                                 struct map_cell *cell = &map->cells[j][i];
 458                                                 color = lightGray;
 459
 460                                                 if (cell->flags & MAP_FLAG_WALL)
 461                                                                 color = darkYellow;
 462                                                 if (cell->flags & MAP_FLAG_IGNORE_OBST)
 463                                                                 color = darkGreen;
 464                                                 if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 465                                                                 color = yellow;
 466                                                 if (cell->flags & MAP_FLAG_PATH)
 467                                                                 color = darkRed;
 468                                                 if (cell->flags & MAP_FLAG_START)
 469                                                                 color = red;
 470                                                 if (cell->flags & MAP_FLAG_GOAL)
 471                                                                 color = green;
 472                         if (cell->flags & MAP_FLAG_PLAN_MARGIN) {
 473                                 QColor c(240, 170, 50); /* orange */
 474                                 color = c;
 475                         }
 476                                                 if (cell->detected_obstacle) {
 477                                                                 QColor c1(color), c2(blue);
 478                                                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 479                                                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 480                                                                                  c1.green() + (int)(f*(c2.green() - c1.green())),
 481                                                                                  c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 482                                                                 color = c;
 483                                                 }
 484                                                 if (cell->flags & MAP_FLAG_DET_OBST)
 485                                                                 color = cyan;
 486
 487                         color.setAlpha(200);
 488                                                 mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
 489                                 }
 490         }
 491 }
 492
 493 void RobomonAtlantis::setHokuyoSimulation(int state)
 494 {
 495         if(state) {
 496                 robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
 497         } else {
 498                 if (!hokuyoSimEnabled)
 499                         return;
 500                 robottype_publisher_hokuyo_scan_destroy(&orte);
 501         }
 502         hokuyoSimEnabled = state;
 503 }
 504
 505 void RobomonAtlantis::setSick331Simulation(int state)
 506 {
 507         if(state) {
 508                 robottype_publisher_sick_scan_create(&orte, NULL, this);
 509         } else {
 510                 if (!sickSimEnabled)
 511                         return;
 512                 robottype_publisher_sick_scan_destroy(&orte);
 513         }
 514         sickSimEnabled = state;
 515 }
 516
 517 void RobomonAtlantis::setSick551Simulation(int state)
 518 {
 519         if(state) {
 520                 robottype_publisher_sick551_scan_create(&orte, NULL, this);
 521         } else {
 522                 if (!sick551SimEnabled)
 523                         return;
 524                 robottype_publisher_sick551_scan_destroy(&orte);
 525         }
 526         sick551SimEnabled = state;
 527 }
 528
 529 void RobomonAtlantis::setHokuyoObstacleSimulation(int state)
 530 {
 531         if (state) {
 532                 /* TODO Maybe it is possible to attach only once to Shmap */
 533                 ShmapInit(0);
 534                 obstacleSimulationTimerHokuyo = new QTimer(this);
 535                 connect(obstacleSimulationTimerHokuyo, SIGNAL(timeout()),
 536                         this, SLOT(simulateObstaclesHokuyo()));
 537                 obstacleSimulationTimerHokuyo->start(100);
 538                 setMouseTracking(true);
 539                 hokuyoScan->setVisible(true);
 540         } else {
 541                 if (obstacleSimulationTimerHokuyo)
 542                         delete obstacleSimulationTimerHokuyo;
 543                 // Hide scans of lidars
 544                 hokuyoScan->setVisible(false);
 545         }
 546 }
 547
 548 void RobomonAtlantis::setSick331ObstacleSimulation(int state)
 549 {
 550         if (state) {
 551                 /* TODO Maybe it is possible to attach only once to Shmap */
 552                 ShmapInit(0);
 553                 obstacleSimulationTimerSick331 = new QTimer(this);
 554                 connect(obstacleSimulationTimerSick331, SIGNAL(timeout()),
 555                         this, SLOT(simulateObstaclesSick()));
 556                 obstacleSimulationTimerSick331->start(100);
 557                 setMouseTracking(true);
 558                 sickScan->setVisible(true);
 559         } else {
 560                 if (obstacleSimulationTimerSick331)
 561                         delete obstacleSimulationTimerSick331;
 562                 // Hide scans of lidars
 563                 sickScan->setVisible(false);
 564         }
 565 }
 566
 567 void RobomonAtlantis::setSick551ObstacleSimulation(int state)
 568 {
 569         if (state) {
 570                 /* TODO Maybe it is possible to attach only once to Shmap */
 571                 ShmapInit(0);
 572                 obstacleSimulationTimerSick551 = new QTimer(this);
 573                 connect(obstacleSimulationTimerSick551, SIGNAL(timeout()),
 574                         this, SLOT(simulateObstaclesSick551()));
 575                 obstacleSimulationTimerSick551->start(100);
 576                 setMouseTracking(true);
 577                 sick551Scan->setVisible(true);
 578         } else {
 579                 if (obstacleSimulationTimerSick551)
 580                         delete obstacleSimulationTimerSick551;
 581                 // Hide scans of lidars
 582                 sick551Scan->setVisible(false);
 583         }
 584 }
 585
 586 void RobomonAtlantis::simulateObstaclesLidar(const struct lidar_params lidar)
 587 {
 588         double distance, wall_distance;
 589         unsigned int i;
 590         unsigned int data_lenght = 0;
 591         uint16_t *lidar_data = NULL;
 592         switch (lidar.type) {
 593                 case HOKUYO:
 594                         lidar_data = orte.hokuyo_scan.data;
 595                         data_lenght = hokuyo_params.data_lenght;
 596                         break;
 597                 case SICK_TIM3XX:
 598                         lidar_data = orte.sick_scan.data;
 599                         data_lenght = sick_params.data_lenght;
 600                         break;
 601                 case SICK_TIM551:
 602                         lidar_data = orte.sick551_scan.data;
 603                         data_lenght = sick551_params.data_lenght;
 604                         break;
 605                 default:
 606                         return;
 607         }
 608
 609         for (i = 0; i < data_lenght; i++) {
 610                 wall_distance = distanceToWallLidar(lidar, i);
 611                 distance = distanceToCircularObstacleLidar(lidar, i, simulatedObstacle, SIM_OBST_SIZE_M);
 612                 if (wall_distance < distance)
 613                         distance = wall_distance;
 614                 lidar_data[i] = distance*1000;
 615         }
 616
 617         switch (lidar.type) {
 618                 case HOKUYO:
 619                         orte.hokuyo_scan.data_lenght = hokuyo_params.data_lenght;
 620                         orte.hokuyo_scan.lidar_type = hokuyo_params.type;
 621                         ORTEPublicationSend(orte.publication_hokuyo_scan);
 622                         break;
 623                 case SICK_TIM3XX:
 624                         orte.sick_scan.data_lenght = sick_params.data_lenght;
 625                         orte.sick_scan.lidar_type = sick_params.type;
 626                         ORTEPublicationSend(orte.publication_sick_scan);
 627                         break;
 628                 case SICK_TIM551:
 629                         orte.sick551_scan.data_lenght = sick551_params.data_lenght;
 630                         orte.sick551_scan.lidar_type = sick551_params.type;
 631                         ORTEPublicationSend(orte.publication_sick551_scan);
 632                         break;
 633                 default:
 634                         return;
 635         }
 636 }
 637
 638 void RobomonAtlantis::simulateObstaclesHokuyo()
 639 {
 640         simulateObstaclesLidar(hokuyo_params);
 641 }
 642
 643 void RobomonAtlantis::simulateObstaclesSick()
 644 {
 645         simulateObstaclesLidar(sick_params);
 646 }
 647
 648 void RobomonAtlantis::simulateObstaclesSick551()
 649 {
 650         simulateObstaclesLidar(sick551_params);
 651 }
 652
 653 void RobomonAtlantis::changeObstacle(QPointF position)
 654 {
 655         if (!hokuyoSimEnabled && !sickSimEnabled && !sick551SimEnabled) {
 656                 hokuyoSimEnabled = 1;
 657                 sickSimEnabled = 1;
 658                 sick551SimEnabled = 1;
 659                 hokuyoSimCheckBox->setChecked(true);
 660                 sick331SimCheckBox->setChecked(true);
 661                 sick551SimCheckBox->setChecked(true);
 662         }
 663
 664         simulatedObstacle.x = position.x();
 665         simulatedObstacle.y = position.y();
 666 }
 667
 668 /**********************************************************************
 669  * EVENTS
 670  **********************************************************************/
 671 bool RobomonAtlantis::event(QEvent *event)
 672 {
 673         switch (event->type()) {
 674                 case QEVENT(QEV_MOTION_STATUS):
 675                         emit motionStatusReceivedSignal();
 676                         break;
 677                 case QEVENT(QEV_SICK_SCAN):
 678                         sickScan->newScan(&orte.sick_scan);
 679                         break;
 680                 case QEVENT(QEV_SICK551_SCAN):
 681                         sick551Scan->newScan(&orte.sick551_scan);
 682                         break;
 683                 case QEVENT(QEV_HOKUYO_SCAN):
 684                         hokuyoScan->newScan(&orte.hokuyo_scan);
 685                         break;
 686                 case QEVENT(QEV_REFERENCE_POSITION):
 687                         emit actualPositionReceivedSignal();
 688                         break;
 689                 case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO):
 690                         estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3));
 691                         estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3));
 692                         estPosPhi->setText(QString("%1(%2)")
 693                                         .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0)
 694                                         .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1));
 695                         robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x,
 696                                 orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi);
 697                         trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x,
 698                                                   orte.est_pos_indep_odo.y));
 699                         break;
 700                 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
 701                         robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
 702                                         orte.est_pos_odo.y, orte.est_pos_odo.phi);
 703                         trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
 704                                                   orte.est_pos_odo.y));
 705                         break;
 706                 case QEVENT(QEV_ESTIMATED_POSITION_BEST):
 707                         robotEstPosBest->moveRobot(orte.est_pos_best.x,
 708                                         orte.est_pos_best.y, orte.est_pos_best.phi);
 709                         trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x,
 710                                                   orte.est_pos_best.y));
 711                         hokuyoScan->setPosition(orte.est_pos_best.x,
 712                                                 orte.est_pos_best.y,
 713                                                 orte.est_pos_best.phi);
 714                         sickScan->setPosition(orte.est_pos_best.x,
 715                                                 orte.est_pos_best.y,
 716                                                 orte.est_pos_best.phi);
 717                         sick551Scan->setPosition(orte.est_pos_best.x,
 718                                                 orte.est_pos_best.y,
 719                                                 orte.est_pos_best.phi);
 720                         break;
 721                 case QEVENT(QEV_POWER_VOLTAGE):
 722                         emit powerVoltageReceivedSignal();
 723                         break;
 724                 case QEVENT(QEV_FSM_MAIN):
 725                         fsm_main_state->setText(orte.fsm_main.state_name);
 726                         break;
 727                 case QEVENT(QEV_FSM_ACT):
 728                         fsm_act_state->setText(orte.fsm_act.state_name);
 729                         break;
 730                 case QEVENT(QEV_FSM_MOTION):
 731                         fsm_motion_state->setText(orte.fsm_motion.state_name);
 732                         break;
 733                 default:
 734                         if (event->type() == QEvent::Close)
 735                                 closeEvent((QCloseEvent *)event);
 736                         else if (event->type() == QEvent::KeyPress)
 737                                 keyPressEvent((QKeyEvent *)event);
 738                         else if (event->type() == QEvent::KeyRelease)
 739                                 keyReleaseEvent((QKeyEvent *)event);
 740                         else if (event->type() == QEvent::FocusIn)
 741                                 grabKeyboard();
 742                         else if (event->type() == QEvent::FocusOut)
 743                                 releaseKeyboard();
 744                         else {
 745                                 event->ignore();
 746                                 return false;
 747                         }
 748                         break;
 749         }
 750         event->accept();
 751         return true;
 752 }
 753
 754 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
 755 {
 756 //      double peak, gain;
 757
 758         if (event->isAutoRepeat()) {
 759                 switch (event->key()) {
 760 //                      case Qt::Key_Down:
 761 //                              peak = leftMotorSlider->minimum()/2;
 762 //                              if (leftMotorValue < peak ||
 763 //                                      rightMotorValue < peak)
 764 //                                      gain = 1.01;
 765 //                              else
 766 //                                      gain = 1.3;
 767 //                              leftMotorValue *= gain;
 768 //                              rightMotorValue *= gain;
 769 //                              leftMotorSlider->setValue((int)leftMotorValue);
 770 //                              rightMotorSlider->setValue((int)rightMotorValue);
 771 //                              break;
 772
 773 //                      case Qt::Key_Up:
 774 //                      case Qt::Key_Left:
 775 //                      case Qt::Key_Right:
 776 //                              peak = leftMotorSlider->maximum()/2;
 777 //                              if (leftMotorValue > peak ||
 778 //                                      rightMotorValue > peak)
 779 //                                      gain = 1.01;
 780 //                              else
 781 //                                      gain = 1.3;
 782 //                              leftMotorValue *= gain;
 783 //                              rightMotorValue *= gain;
 784 //                              leftMotorSlider->setValue((int)leftMotorValue);
 785 //                              rightMotorSlider->setValue((int)rightMotorValue);
 786 //                              break;
 787
 788                         default:
 789                                 event->ignore();
 790                                 break;
 791                 }
 792                 return;
 793         }
 794
 795         switch (event->key()) {
 796 //              case Qt::Key_Up:
 797 //                      leftMotorValue = 1;
 798 //                      rightMotorValue = 1;
 799 //                      bothMotorsCheckBox->setChecked(true);
 800 //                      leftMotorSlider->setValue((int)leftMotorValue);
 801 //                      setLeftMotor((int)leftMotorValue);
 802 //                      break;
 803 //              case Qt::Key_Down:
 804 //                      leftMotorValue = -1;
 805 //                      rightMotorValue = -1;
 806 //                      bothMotorsCheckBox->setChecked(true);
 807 //                      leftMotorSlider->setValue((int)leftMotorValue);
 808 //                      setLeftMotor((int)leftMotorValue);
 809 //                      break;
 810 //              case Qt::Key_Left:
 811 //                      leftMotorValue = -1;
 812 //                      rightMotorValue = 1;
 813 //                      leftMotorSlider->setValue((int)leftMotorValue);
 814 //                      rightMotorSlider->setValue((int)rightMotorValue);
 815 //                      setLeftMotor((int)leftMotorValue);
 816 //                      setRightMotor((int)leftMotorValue);
 817 //                      break;
 818 //              case Qt::Key_Right:
 819 //                      leftMotorValue = 1;
 820 //                      rightMotorValue = -1;
 821 //                      leftMotorSlider->setValue((int)leftMotorValue);
 822 //                      rightMotorSlider->setValue((int)rightMotorValue);
 823 //                      setLeftMotor((int)leftMotorValue);
 824 //                      setRightMotor((int)rightMotorValue);
 825 //                      break;
 826                 default:
 827                         event->ignore();
 828                         break;
 829         }
 830         event->accept();
 831 }
 832
 833 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
 834 {
 835         if (event->isAutoRepeat()) {
 836                 event->ignore();
 837                 return;
 838         }
 839
 840         switch (event->key()) {
 841 //              case Qt::Key_Up:
 842 //              case Qt::Key_Down:
 843 //              case Qt::Key_Left:
 844 //              case Qt::Key_Right:
 845 //                      leftMotorValue = 0;
 846 //                      rightMotorValue = 0;
 847 //                      bothMotorsCheckBox->setChecked(false);
 848 //                      leftMotorSlider->setValue((int)leftMotorValue);
 849 //                      rightMotorSlider->setValue((int)rightMotorValue);
 850 //                      break;
 851                 default:
 852                         event->ignore();
 853                         break;
 854         }
 855         event->accept();
 856 }
 857
 858 void RobomonAtlantis::closeEvent(QCloseEvent *)
 859 {
 860         robottype_roboorte_destroy(&orte);
 861 }
 862
 863 /**********************************************************************
 864  * ORTE
 865  **********************************************************************/
 866 void RobomonAtlantis::createOrte()
 867 {
 868         int rv;
 869
 870         memset(&orte, 0, sizeof(orte));
 871         rv = robottype_roboorte_init(&orte);
 872         if (rv) {
 873                 printf("RobomonAtlantis: Unable to initialize ORTE\n");
 874         }
 875
 876         /* publishers */
 877         robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
 878
 879         robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
 880         robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
 881         robottype_publisher_robot_switches_create(&orte, dummy_publisher_callback, &orte);
 882
 883         /* subscribers */
 884         robottype_subscriber_pwr_voltage_create(&orte,
 885                                 receivePowerVoltageCallBack, this);
 886         robottype_subscriber_motion_status_create(&orte,
 887                                 receiveMotionStatusCallBack, this);
 888         robottype_subscriber_ref_pos_create(&orte,
 889                                 receiveActualPositionCallBack, this);
 890         robottype_subscriber_est_pos_odo_create(&orte,
 891                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
 892         robottype_subscriber_est_pos_indep_odo_create(&orte,
 893                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO));
 894         robottype_subscriber_est_pos_best_create(&orte,
 895                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST));
 896         robottype_subscriber_sick_scan_create(&orte,
 897                                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK_SCAN));
 898         robottype_subscriber_sick551_scan_create(&orte,
 899                                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK551_SCAN));
 900         robottype_subscriber_hokuyo_scan_create(&orte,
 901                                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
 902         robottype_subscriber_fsm_main_create(&orte,
 903                                                  rcv_fsm_main_cb, this);
 904         robottype_subscriber_fsm_motion_create(&orte,
 905                                                  rcv_fsm_motion_cb, this);
 906         robottype_subscriber_fsm_act_create(&orte,
 907                                                  rcv_fsm_act_cb, this);
 908
 909         /* motors */
 910         orte.motion_speed.left = 0;
 911         orte.motion_speed.right = 0;
 912
 913         /* power management */
 914                 orte.pwr_ctrl.voltage33 = true;
 915                 orte.pwr_ctrl.voltage50 = true;
 916                 orte.pwr_ctrl.voltage80 = true;
 917         voltage33CheckBox->setChecked(true);
 918         voltage50CheckBox->setChecked(true);
 919         voltage80CheckBox->setChecked(true);
 920
 921         act_init(&orte);
 922
 923         /* set actions to do when we receive data from orte */
 924         connect(this, SIGNAL(motionStatusReceivedSignal()),
 925                         this, SLOT(motionStatusReceived()));
 926         connect(this, SIGNAL(actualPositionReceivedSignal()),
 927                         this, SLOT(actualPositionReceived()));
 928         connect(this, SIGNAL(powerVoltageReceivedSignal()),
 929                         this, SLOT(powerVoltageReceived()));
 930 }
 931
 932 void RobomonAtlantis::motionStatusReceived()
 933 {
 934         WDBG("ORTE received: motion status");
 935 }
 936
 937 void RobomonAtlantis::actualPositionReceived()
 938 {
 939         actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
 940         actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
 941         actPosPhi->setText(QString("%1(%2)")
 942                         .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
 943                         .arg(orte.ref_pos.phi, 0, 'f', 1));
 944         robotRefPos->moveRobot(orte.ref_pos.x,
 945                 orte.ref_pos.y, orte.ref_pos.phi);
 946         trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
 947 }
 948
 949 void RobomonAtlantis::powerVoltageReceived()
 950 {
 951         voltage33LineEdit->setText(QString("%1").arg(
 952                         orte.pwr_voltage.voltage33, 0, 'f', 3));
 953         voltage50LineEdit->setText(QString("%1").arg(
 954                         orte.pwr_voltage.voltage50, 0, 'f', 3));
 955         voltage80LineEdit->setText(QString("%1").arg(
 956                         orte.pwr_voltage.voltage80, 0, 'f', 3));
 957         voltageBATLineEdit->setText(QString("%1").arg(
 958                         orte.pwr_voltage.voltageBAT, 0, 'f', 3));
 959
 960 }
 961
 962 /**********************************************************************
 963  * MISCELLANEOUS
 964  **********************************************************************/
 965 void RobomonAtlantis::openSharedMemory()
 966 {
 967         int segmentId;
 968         int sharedSegmentSize;
 969
 970         if (sharedMemoryOpened)
 971                 return;
 972
 973         sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
 974
 975         /* Get segment identificator in a read only mode  */
 976         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
 977         if(segmentId == -1) {
 978                 statusBar->showMessage("No external map found - creating a new map.");
 979         }
 980
 981         /* Init Shmap */
 982         ShmapInit(0);
 983
 984         /* Attach the shared memory segment */
 985         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
 986
 987         sharedMemoryOpened = true;
 988 }
 989
 990 double RobomonAtlantis::distanceToWallLidar(const struct lidar_params lidar, int beamnum)
 991 {
 992         double distance = 4.0, min_distance = 4.0;
 993         int i, j;
 994         Point wall;
 995         struct map *map = ShmapIsMapInit();
 996
 997         if (!map)
 998                 return min_distance;
 999
1000         // Simulate obstacles
1001         for(j = 0; j < MAP_HEIGHT; j++) {
1002                 for (i = 0; i < MAP_WIDTH; i++) {
1003                         struct map_cell *cell = &map->cells[j][i];
1004                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
1005                                 // WALL
1006                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
1007
1008                                 distance = distanceToObstacleLidar(lidar, beamnum, wall, MAP_CELL_SIZE_M);
1009                                 if (distance < min_distance)
1010                                         min_distance = distance;
1011                         }
1012                 }
1013         }
1014
1015         return min_distance;
1016 }
1017
1018 double RobomonAtlantis::distanceToCircularObstacleLidar(const struct lidar_params lidar, int beamnum, Point center, double diameter)
1019 {
1020         struct robot_pos_type e = orte.est_pos_best;
1021         double sensor_a;
1022         struct sharp_pos s;
1023
1024         s.x = lidar.center_offset_m;
1025         s.y = 0.0;
1026         s.ang = index2rad(lidar, beamnum);
1027
1028         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
1029                          e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
1030         sensor_a = e.phi + s.ang;
1031
1032         const double sensorRange = 4.0; /*[meters]*/
1033
1034         double distance = sensorRange;
1035         double angle;
1036
1037         angle = sensor.angleTo(center) - sensor_a;
1038         angle = fmod(angle, 2.0 * M_PI);
1039         if (angle > +M_PI) angle -= 2.0 * M_PI;
1040         if (angle < -M_PI) angle += 2.0 * M_PI;
1041         angle = fabs(angle);
1042
1043         double k = tan(sensor_a);
1044         double r = diameter / 2.0;
1045
1046         double A = 1 + k * k;
1047         double B = 2 * (sensor.y * k - center.x - k * k * sensor.x - center.y * k);
1048         double C = center.x * center.x + center.y * center.y +
1049                 k * k * sensor.x * sensor.x - 2*sensor.y*k*sensor.x +
1050                 sensor.y * sensor.y + 2 * k * sensor.x *center.y -
1051                 2 * sensor.y * center.y - r * r;
1052
1053         double D = B * B - 4 * A * C;
1054
1055         if (D > 0) {
1056                 Point ob1, ob2;
1057
1058                 ob1.x = (-B + sqrt(D)) / (2 * A);
1059                 ob2.x = (-B - sqrt(D)) / (2 * A);
1060                 ob1.y = k * (ob1.x - sensor.x) + sensor.y;
1061                 ob2.y = k * (ob2.x - sensor.x) + sensor.y;
1062
1063                 double distance1 = sensor.distanceTo(ob1);
1064                 double distance2 = sensor.distanceTo(ob2);
1065                 distance = (distance1 < distance2) ? distance1 : distance2;
1066         } else if (D == 0) {
1067                 Point ob;
1068                 ob.x = -B / (2 * A);
1069                 ob.y = k * (ob.x - sensor.x) + sensor.y;
1070                 distance = sensor.distanceTo(ob);
1071         }
1072         distance = distance + (drand48() - 0.5) * 3.0e-2;
1073         if (D < 0 || angle > atan(r / distance))
1074                 distance = sensorRange;
1075         if (distance > sensorRange)
1076                 distance = sensorRange;
1077
1078         return distance;
1079 }
1080
1081 /**
1082  * Calculation for Lidar simulation. Calculates distance that would
1083  * be returned by Lidar sensors, if there is only one obstacle (as
1084  * specified by parameters).
1085  *
1086  * @param beamnum Lidar's bean number [0..LIDAR_CLUSTER_CNT]
1087  * @param obstacle Position of the obstacle (x, y in meters).
1088  * @param obstacleSize Size (diameter) of the obstacle in meters.
1089  *
1090  * @return Distance measured by sensors in meters.
1091  */
1092 double RobomonAtlantis::distanceToObstacleLidar(const struct lidar_params lidar, int beamnum, Point obstacle, double obstacleSize)
1093 {
1094         struct robot_pos_type e = orte.est_pos_best;
1095         double sensor_a;
1096         struct sharp_pos s;
1097
1098         s.x = lidar.center_offset_m;
1099         s.y = 0.0;
1100         s.ang = index2rad(lidar, beamnum);
1101
1102         Point sensor(e.x + s.x * cos(e.phi) - s.y * sin(e.phi),
1103                          e.y + s.x * sin(e.phi) + s.y * cos(e.phi));
1104         sensor_a = e.phi + s.ang;
1105
1106         const double sensorRange = 4.0; /*[meters]*/
1107
1108         double distance, angle;
1109
1110         angle = sensor.angleTo(obstacle) - sensor_a;
1111         angle = fmod(angle, 2.0 * M_PI);
1112         if (angle > +M_PI) angle -= 2.0 * M_PI;
1113         if (angle < -M_PI) angle += 2.0 * M_PI;
1114         angle = fabs(angle);
1115         distance = sensor.distanceTo(obstacle) - obstacleSize/2.0;
1116         if (angle < atan(obstacleSize/2.0 / distance)) {
1117                 // We can see the obstackle from here.
1118                 if (angle < M_PI/2.0) {
1119                         distance = distance / cos(angle);
1120                 }
1121                 if (distance > sensorRange)
1122                         distance = sensorRange;
1123         } else {
1124                 distance = sensorRange;
1125         }
1126
1127         return distance;
1128 }
1129
1130 void RobomonAtlantis::sendStart(int plug)
1131 {
1132         orte.robot_cmd.start_condition = plug ? 0 : 1;
1133         ORTEPublicationSend(orte.publication_robot_cmd);
1134 }
1135
1136 void RobomonAtlantis::setTeamColor(int plug)
1137 {
1138         orte.robot_switches.team_color = plug ? 1 : 0;
1139         ORTEPublicationSend(orte.publication_robot_switches);
1140 }
1141
1142 void RobomonAtlantis::resetTrails()
1143 {
1144         trailRefPos->reset();
1145         trailEstPosBest->reset();
1146         trailPosIndepOdo->reset();
1147         trailOdoPos->reset();
1148 }
1149
1150 void RobomonAtlantis::showTrails(bool show)
1151 {
1152         trailRefPos->setVisible(show && robotRefPos->isVisible());
1153         trailEstPosBest->setVisible(show && robotEstPosBest->isVisible());
1154         trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible());
1155         trailOdoPos->setVisible(show && robotEstPosOdo->isVisible());
1156 }
1157
1158 void RobomonAtlantis::showShapeDetect(bool show)
1159 {
1160         hokuyoScan->showShapeDetect = show;
1161 }