X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/12449dd6e237f58002b167d2e28af61d4b5b145a..d3f518a5fcbbd8c4fe0a19c20d6a4162403b8ae0:/src/robomon/RobomonAtlantis.cpp diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp index 2926a722..7995af1d 100644 --- a/src/robomon/RobomonAtlantis.cpp +++ b/src/robomon/RobomonAtlantis.cpp @@ -65,6 +65,10 @@ RobomonAtlantis::RobomonAtlantis(QWidget *parent) createOrte(); createRobots(); createActions(); + createMap(); + +// connect(vidle, SIGNAL(valueChanged(int)), +// robotEstPosBest, SLOT(setVidle(int))); setFocusPolicy(Qt::StrongFocus); sharedMemoryOpened = false; @@ -77,34 +81,29 @@ RobomonAtlantis::RobomonAtlantis(QWidget *parent) void RobomonAtlantis::createLeftLayout() { leftLayout = new QVBoxLayout(); - + createDebugGroupBox(); debugWindowEnabled = true; createPlaygroundGroupBox(); leftLayout->addWidget(playgroundGroupBox); - leftLayout->addWidget(debugGroupBox); + //leftLayout->addWidget(debugGroupBox); // FIXME: move this to separate tab } void RobomonAtlantis::createRightLayout() { rightLayout = new QVBoxLayout(); - QGridLayout *layout = new QGridLayout(); - QVBoxLayout *vlayout = new QVBoxLayout(); - + createPositionGroupBox(); createMiscGroupBox(); createFSMGroupBox(); createActuatorsGroupBox(); createPowerGroupBox(); - vlayout->addWidget(positionGroupBox); - vlayout->addWidget(miscGroupBox); - vlayout->addWidget(fsmGroupBox); - layout->addLayout(vlayout, 0, 0); - layout->addWidget(actuatorsGroupBox, 0, 1); - - rightLayout->addLayout(layout); + rightLayout->addWidget(positionGroupBox); + rightLayout->addWidget(miscGroupBox); + rightLayout->addWidget(fsmGroupBox); rightLayout->addWidget(powerGroupBox); + rightLayout->addWidget(actuatorsGroupBox); } void RobomonAtlantis::createPlaygroundGroupBox() @@ -119,6 +118,7 @@ void RobomonAtlantis::createPlaygroundGroupBox() playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true); playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect()); playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + playgroundSceneView->setMouseTracking(true); layout->addWidget(playgroundSceneView); playgroundGroupBox->setLayout(layout); @@ -127,9 +127,9 @@ void RobomonAtlantis::createPlaygroundGroupBox() void RobomonAtlantis::createPositionGroupBox() { positionGroupBox = new QGroupBox(tr("Position state")); - positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); QGridLayout *layout = new QGridLayout(); - + actPosX = new QLineEdit(); actPosY = new QLineEdit(); actPosPhi = new QLineEdit(); @@ -137,7 +137,7 @@ void RobomonAtlantis::createPositionGroupBox() estPosX = new QLineEdit(); estPosY = new QLineEdit(); estPosPhi = new QLineEdit(); - + actPosX->setReadOnly(true); actPosY->setReadOnly(true); actPosPhi->setReadOnly(true); @@ -159,7 +159,7 @@ void RobomonAtlantis::createPositionGroupBox() layout->addWidget(actPosY, 2, 1); layout->addWidget(actPosPhi, 3, 1); - layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1); + layout->addWidget(MiscGui::createLabel("Estimated (indep. odo.)", Qt::AlignLeft), 4, 1); layout->addWidget(estPosX, 5, 1); layout->addWidget(estPosY, 6, 1); layout->addWidget(estPosPhi, 7, 1); @@ -177,12 +177,15 @@ void RobomonAtlantis::createMiscGroupBox() obstacleSimulationCheckBox->setShortcut(tr("o")); layout->addWidget(obstacleSimulationCheckBox); - startPlug = new QCheckBox("Start plug"); + startPlug = new QCheckBox("&Start plug"); layout->addWidget(startPlug); - - puckInside = new QCheckBox("Puck inside"); - layout->addWidget(puckInside); - + + colorChoser = new QCheckBox("&Team color"); + layout->addWidget(colorChoser); + + strategyButton= new QPushButton(tr("Strategy")); + layout->addWidget(strategyButton); + miscGroupBox->setLayout(layout); } @@ -195,6 +198,7 @@ void RobomonAtlantis::createFSMGroupBox() layout->addWidget(MiscGui::createLabel("Main:"), 1, 0); fsm_main_state = new QLabel(); fsm_main_state->setMinimumWidth(100); + fsm_main_state->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); layout->addWidget(fsm_main_state, 1, 1); layout->addWidget(MiscGui::createLabel("Act:"), 2, 0); @@ -226,19 +230,24 @@ void RobomonAtlantis::createActuatorsGroupBox() actuatorsGroupBox = new QGroupBox(tr("Actuators")); actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred); QHBoxLayout *layout = new QHBoxLayout(); +// vidle = new QDial(); - createMotorsGroupBox(); +// vidle->setMinimum(VIDLE_VYSIP); +// vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP); +// vidle->setEnabled(true); + + //createMotorsGroupBox(); layout->setAlignment(Qt::AlignLeft); - layout->addWidget(enginesGroupBox); +// layout->addWidget(vidle); + //layout->addWidget(enginesGroupBox); actuatorsGroupBox->setLayout(layout); } void RobomonAtlantis::createPowerGroupBox() { powerGroupBox = new QGroupBox(tr("Power management")); - powerGroupBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum); - powerGroupBox->setMaximumWidth(450); + powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); QGridLayout *layout = new QGridLayout(); voltage33CheckBox = new QCheckBox(tr("&3.3V")); @@ -250,9 +259,9 @@ void RobomonAtlantis::createPowerGroupBox() voltage80CheckBox->setShortcut(tr("8")); layout->addWidget(voltage33CheckBox, 0, 0); - layout->addWidget(voltage50CheckBox, 0, 2); - layout->addWidget(voltage80CheckBox, 0, 4); //1, 0); - layout->addWidget(MiscGui::createLabel("BAT"), 0, 6); //1, 2); + layout->addWidget(voltage50CheckBox, 1, 0); + layout->addWidget(voltage80CheckBox, 2, 0); + layout->addWidget(MiscGui::createLabel("BAT"), 3, 0); voltage33LineEdit = new QLineEdit(); voltage50LineEdit = new QLineEdit(); @@ -265,76 +274,60 @@ void RobomonAtlantis::createPowerGroupBox() voltageBATLineEdit->setReadOnly(true); layout->addWidget(voltage33LineEdit, 0, 1); - layout->addWidget(voltage50LineEdit, 0, 3); - layout->addWidget(voltage80LineEdit, 0, 5); //1, 1); - layout->addWidget(voltageBATLineEdit, 0, 7); //1, 3); + layout->addWidget(voltage50LineEdit, 1, 1); + layout->addWidget(voltage80LineEdit, 2, 1); + layout->addWidget(voltageBATLineEdit, 3, 1); powerGroupBox->setLayout(layout); } -void RobomonAtlantis::createMotorsGroupBox() +void RobomonAtlantis::createRobots() { - enginesGroupBox = new QGroupBox(tr("Motors")); - QVBoxLayout *layout = new QVBoxLayout(); - QHBoxLayout *layout1 = new QHBoxLayout(); - QHBoxLayout *layout2 = new QHBoxLayout(); + robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush)); + robotRefPos->setZValue(11); + trailRefPos = new Trail(QPen(Qt::darkBlue)); + trailRefPos->setZValue(11); - leftMotorSlider = new QSlider(Qt::Vertical); - rightMotorSlider = new QSlider(Qt::Vertical); - bothMotorsCheckBox = new QCheckBox(tr("Lock both")); - stopMotorsPushButton = new QPushButton(tr("Stop Motors")); + robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray)); + robotEstPosBest->setZValue(10); + trailEstPosBest = new Trail(QPen()); + trailEstPosBest->setZValue(10); - leftMotorSlider->setMinimum(-100); - leftMotorSlider->setMaximum(100); - leftMotorSlider->setTracking(false); - leftMotorSlider->setTickPosition(QSlider::TicksLeft); + robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed)); + robotEstPosOdo->setZValue(10); + trailOdoPos = new Trail(QPen(Qt::red)); + trailOdoPos->setZValue(10); - rightMotorSlider->setMinimum(-100); - rightMotorSlider->setMaximum(100); - rightMotorSlider->setTracking(false); - rightMotorSlider->setTickPosition(QSlider::TicksRight); + robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen)); + robotEstPosIndepOdo->setZValue(10); + trailPosIndepOdo = new Trail(QPen(Qt::green)); + trailPosIndepOdo->setZValue(10); - stopMotorsPushButton->setMaximumWidth(90); + playgroundScene->addItem(robotRefPos); + playgroundScene->addItem(robotEstPosBest); + playgroundScene->addItem(robotEstPosIndepOdo); + playgroundScene->addItem(robotEstPosOdo); - layout1->addWidget(leftMotorSlider); - layout1->addWidget(MiscGui::createLabel("0")); - layout1->addWidget(rightMotorSlider); + showTrails(false); - layout2->addWidget(bothMotorsCheckBox); + playgroundScene->addItem(trailRefPos); + playgroundScene->addItem(trailPosIndepOdo); + playgroundScene->addItem(trailOdoPos); + + hokuyoScan = new HokuyoScan(); + hokuyoScan->setZValue(10); + playgroundScene->addItem(hokuyoScan); - layout->addWidget(MiscGui::createLabel("100")); - layout->addLayout(layout1); - layout->addWidget(MiscGui::createLabel("-100")); - layout->addLayout(layout2); - layout->addWidget(stopMotorsPushButton); - enginesGroupBox->setLayout(layout); } -void RobomonAtlantis::createRobots() +void RobomonAtlantis::createMap() { - robotActPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush)); - robotActPos->setZValue(11); - trailRefPos = new Trail(QPen(Qt::darkBlue)); - trailRefPos->setZValue(11); - - robotEstPosOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkRed)); - robotEstPosOdo->setZValue(10); - trailOdoPos = new Trail(QPen(Qt::white)); - trailOdoPos->setZValue(10); - robotEstPosUzv = new Robot("Uzv", QPen(), QBrush(Qt::darkGray)); - robotEstPosUzv->setZValue(10); - trailUzvPos = new Trail(QPen()); - trailUzvPos->setZValue(10); + mapImage = new Map(); + mapImage->setZValue(5); + mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true); - playgroundScene->addItem(robotActPos); - playgroundScene->addItem(robotEstPosUzv); - playgroundScene->addItem(robotEstPosOdo); - - showTrails(false); - - playgroundScene->addItem(trailRefPos); - playgroundScene->addItem(trailUzvPos); - playgroundScene->addItem(trailOdoPos); + + playgroundScene->addItem(mapImage); } /********************************************************************** @@ -343,24 +336,26 @@ void RobomonAtlantis::createRobots() void RobomonAtlantis::createActions() { /* power management */ - connect(voltage33CheckBox, SIGNAL(stateChanged(int)), + connect(voltage33CheckBox, SIGNAL(stateChanged(int)), this, SLOT(setVoltage33(int))); - connect(voltage50CheckBox, SIGNAL(stateChanged(int)), + connect(voltage50CheckBox, SIGNAL(stateChanged(int)), this, SLOT(setVoltage50(int))); - connect(voltage80CheckBox, SIGNAL(stateChanged(int)), + connect(voltage80CheckBox, SIGNAL(stateChanged(int)), this, SLOT(setVoltage80(int))); /* motors */ - connect(leftMotorSlider, SIGNAL(valueChanged(int)), - this, SLOT(setLeftMotor(int))); - connect(rightMotorSlider, SIGNAL(valueChanged(int)), - this, SLOT(setRightMotor(int))); - connect(stopMotorsPushButton, SIGNAL(clicked()), - this, SLOT(stopMotors())); +// connect(leftMotorSlider, SIGNAL(valueChanged(int)), +// this, SLOT(setLeftMotor(int))); +// connect(rightMotorSlider, SIGNAL(valueChanged(int)), +// this, SLOT(setRightMotor(int))); +// connect(stopMotorsPushButton, SIGNAL(clicked()), +// this, SLOT(stopMotors())); connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int))); - connect(puckInside, SIGNAL(stateChanged(int)), this, SLOT(sendPuckInside(int))); - + connect(colorChoser, SIGNAL(stateChanged(int)), this, SLOT(setTeamColor(int))); + connect(strategyButton, SIGNAL(pressed()), this, SLOT(changeStrategy_1())); + connect(strategyButton, SIGNAL(released()), this, SLOT(changeStrategy_0())); + /* obstacle simulation */ simulationEnabled = 0; connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)), @@ -369,10 +364,22 @@ void RobomonAtlantis::createActions() this, SLOT(setObstacleSimulation(int))); connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)), playgroundScene, SLOT(showObstacle(int))); - connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)), + connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)), this, SLOT(changeObstacle(QPointF))); } +void RobomonAtlantis::changeStrategy_1() +{ + orte.robot_switches.strategy = true; + ORTEPublicationSend(orte.publication_robot_switches); +} + +void RobomonAtlantis::changeStrategy_0() +{ + orte.robot_switches.strategy = false; + ORTEPublicationSend(orte.publication_robot_switches); +} + void RobomonAtlantis::setVoltage33(int state) { if (state) @@ -397,42 +404,47 @@ void RobomonAtlantis::setVoltage80(int state) orte.pwr_ctrl.voltage80 = false; } -void RobomonAtlantis::setLeftMotor(int value) -{ - short int leftMotor; - short int rightMotor; - - if(bothMotorsCheckBox->isChecked()) - rightMotorSlider->setValue(value); +// void RobomonAtlantis::setLeftMotor(int value) +// { +// short int leftMotor; +// short int rightMotor; - leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0)); - rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0)); - - orte.motion_speed.left = leftMotor; - orte.motion_speed.right = rightMotor; - -} +// if(bothMotorsCheckBox->isChecked()) +// rightMotorSlider->setValue(value); -void RobomonAtlantis::setRightMotor(int value) -{ - short int leftMotor; - short int rightMotor; - - if(bothMotorsCheckBox->isChecked()) - leftMotorSlider->setValue(value); +// leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0)); +// rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0)); - leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0)); - rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0)); - - orte.motion_speed.left = leftMotor; - orte.motion_speed.right = rightMotor; - -} +// orte.motion_speed.left = leftMotor; +// orte.motion_speed.right = rightMotor; + +// } + +// void RobomonAtlantis::setRightMotor(int value) +// { +// short int leftMotor; +// short int rightMotor; + +// if(bothMotorsCheckBox->isChecked()) +// leftMotorSlider->setValue(value); + +// leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0)); +// rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0)); + +// orte.motion_speed.left = leftMotor; +// orte.motion_speed.right = rightMotor; + +// } + +// void RobomonAtlantis::stopMotors() +// { +// leftMotorSlider->setValue(0); +// rightMotorSlider->setValue(0); +// } -void RobomonAtlantis::stopMotors() +void RobomonAtlantis::useOpenGL(bool use) { - leftMotorSlider->setValue(0); - rightMotorSlider->setValue(0); + playgroundSceneView->useOpenGL(&use); } void RobomonAtlantis::showMap(bool show) @@ -441,16 +453,18 @@ void RobomonAtlantis::showMap(bool show) if (sharedMemoryOpened == false) return; - + if (show) { mapTimer = new QTimer(this); connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap())); mapTimer->start(200); } else { - mapTimer->stop(); - disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap())); + if(mapTimer != NULL) { + mapTimer->stop(); + disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap())); + } } - playgroundScene->showMap(show); + mapImage->setVisible(show); } void RobomonAtlantis::paintMap() @@ -459,11 +473,11 @@ void RobomonAtlantis::paintMap() struct map *map = ShmapIsMapInit(); if (!map) return; - - for(int i=0; i < MAP_WIDTH; i++) { - for(int j=0; jcells[j][i]; color = lightGray; @@ -480,6 +494,10 @@ void RobomonAtlantis::paintMap() color = red; if (cell->flags & MAP_FLAG_GOAL) color = green; + if (cell->flags & MAP_FLAG_PLAN_MARGIN) { + QColor c(240, 170, 50); /* orange */ + color = c; + } if (cell->detected_obstacle) { QColor c1(color), c2(blue); double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7; @@ -490,17 +508,17 @@ void RobomonAtlantis::paintMap() } if (cell->flags & MAP_FLAG_DET_OBST) color = cyan; - - playgroundScene->setMapColor(i, j, color); + + color.setAlpha(200); + mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color); } } } void RobomonAtlantis::setSimulation(int state) { - if(state) { - robottype_publisher_hokuyo_scan_create(&orte, - dummy_publisher_callback, this); + if(state) { + robottype_publisher_hokuyo_scan_create(&orte, NULL, this); } else { if (!simulationEnabled) return; @@ -509,7 +527,7 @@ void RobomonAtlantis::setSimulation(int state) simulationEnabled = state; } -/*! +/*! \fn RobomonAtlantis::setObstacleSimulation(int state) */ void RobomonAtlantis::setObstacleSimulation(int state) @@ -518,12 +536,12 @@ void RobomonAtlantis::setObstacleSimulation(int state) /* TODO Maybe it is possible to attach only once to Shmap */ ShmapInit(0); obstacleSimulationTimer = new QTimer(this); - connect(obstacleSimulationTimer, SIGNAL(timeout()), + connect(obstacleSimulationTimer, SIGNAL(timeout()), this, SLOT(simulateObstaclesHokuyo())); obstacleSimulationTimer->start(100); setMouseTracking(true); } else { - if (obstacleSimulationTimer) + if (obstacleSimulationTimer) delete obstacleSimulationTimer; //double distance = 0.8; } @@ -533,18 +551,19 @@ void RobomonAtlantis::setObstacleSimulation(int state) void RobomonAtlantis::simulateObstaclesHokuyo() { double distance, wall_distance; - int i; + unsigned int i; uint16_t *hokuyo = orte.hokuyo_scan.data; - - for (i=0; inewScan(&orte.hokuyo_scan); + break; + case QEVENT(QEV_JAWS_CMD): + robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left); + robotRefPos->setJaws(orte.jaws_cmd.req_pos.left); + robotEstPosIndepOdo->setJaws(orte.jaws_cmd.req_pos.left); + robotEstPosOdo->setJaws(orte.jaws_cmd.req_pos.left); + break; case QEVENT(QEV_REFERENCE_POSITION): emit actualPositionReceivedSignal(); break; - case QEVENT(QEV_ESTIMATED_POSITION_UZV): - emit estimatedPositionReceivedSignal(); + case QEVENT(QEV_ESTIMATED_POSITION_INDEP_ODO): + estPosX->setText(QString("%1").arg(orte.est_pos_indep_odo.x, 0, 'f', 3)); + estPosY->setText(QString("%1").arg(orte.est_pos_indep_odo.y, 0, 'f', 3)); + estPosPhi->setText(QString("%1(%2)") + .arg(DEGREES(orte.est_pos_indep_odo.phi), 0, 'f', 0) + .arg(orte.est_pos_indep_odo.phi, 0, 'f', 1)); + robotEstPosIndepOdo->moveRobot(orte.est_pos_indep_odo.x, + orte.est_pos_indep_odo.y, orte.est_pos_indep_odo.phi); + trailPosIndepOdo->addPoint(QPointF(orte.est_pos_indep_odo.x, + orte.est_pos_indep_odo.y)); break; case QEVENT(QEV_ESTIMATED_POSITION_ODO): - robotEstPosOdo->moveRobot(orte.est_pos_odo.x, + robotEstPosOdo->moveRobot(orte.est_pos_odo.x, orte.est_pos_odo.y, orte.est_pos_odo.phi); - trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x, + trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x, orte.est_pos_odo.y)); break; + case QEVENT(QEV_ESTIMATED_POSITION_BEST): + robotEstPosBest->moveRobot(orte.est_pos_best.x, + orte.est_pos_best.y, orte.est_pos_best.phi); + trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x, + orte.est_pos_best.y)); + hokuyoScan->setPosition(orte.est_pos_best.x, + orte.est_pos_best.y, + orte.est_pos_best.phi); + break; case QEVENT(QEV_POWER_VOLTAGE): emit powerVoltageReceivedSignal(); break; @@ -615,37 +660,37 @@ bool RobomonAtlantis::event(QEvent *event) void RobomonAtlantis::keyPressEvent(QKeyEvent *event) { - double peak, gain; +// double peak, gain; if (event->isAutoRepeat()) { switch (event->key()) { - case Qt::Key_Down: - peak = leftMotorSlider->minimum()/2; - if (leftMotorValue < peak || - rightMotorValue < peak) - gain = 1.01; - else - gain = 1.3; - leftMotorValue *= gain; - rightMotorValue *= gain; - leftMotorSlider->setValue((int)leftMotorValue); - rightMotorSlider->setValue((int)rightMotorValue); - break; - - case Qt::Key_Up: - case Qt::Key_Left: - case Qt::Key_Right: - peak = leftMotorSlider->maximum()/2; - if (leftMotorValue > peak || - rightMotorValue > peak) - gain = 1.01; - else - gain = 1.3; - leftMotorValue *= gain; - rightMotorValue *= gain; - leftMotorSlider->setValue((int)leftMotorValue); - rightMotorSlider->setValue((int)rightMotorValue); - break; +// case Qt::Key_Down: +// peak = leftMotorSlider->minimum()/2; +// if (leftMotorValue < peak || +// rightMotorValue < peak) +// gain = 1.01; +// else +// gain = 1.3; +// leftMotorValue *= gain; +// rightMotorValue *= gain; +// leftMotorSlider->setValue((int)leftMotorValue); +// rightMotorSlider->setValue((int)rightMotorValue); +// break; + +// case Qt::Key_Up: +// case Qt::Key_Left: +// case Qt::Key_Right: +// peak = leftMotorSlider->maximum()/2; +// if (leftMotorValue > peak || +// rightMotorValue > peak) +// gain = 1.01; +// else +// gain = 1.3; +// leftMotorValue *= gain; +// rightMotorValue *= gain; +// leftMotorSlider->setValue((int)leftMotorValue); +// rightMotorSlider->setValue((int)rightMotorValue); +// break; default: event->ignore(); @@ -655,36 +700,36 @@ void RobomonAtlantis::keyPressEvent(QKeyEvent *event) } switch (event->key()) { - case Qt::Key_Up: - leftMotorValue = 1; - rightMotorValue = 1; - bothMotorsCheckBox->setChecked(true); - leftMotorSlider->setValue((int)leftMotorValue); - setLeftMotor((int)leftMotorValue); - break; - case Qt::Key_Down: - leftMotorValue = -1; - rightMotorValue = -1; - bothMotorsCheckBox->setChecked(true); - leftMotorSlider->setValue((int)leftMotorValue); - setLeftMotor((int)leftMotorValue); - break; - case Qt::Key_Left: - leftMotorValue = -1; - rightMotorValue = 1; - leftMotorSlider->setValue((int)leftMotorValue); - rightMotorSlider->setValue((int)rightMotorValue); - setLeftMotor((int)leftMotorValue); - setRightMotor((int)leftMotorValue); - break; - case Qt::Key_Right: - leftMotorValue = 1; - rightMotorValue = -1; - leftMotorSlider->setValue((int)leftMotorValue); - rightMotorSlider->setValue((int)rightMotorValue); - setLeftMotor((int)leftMotorValue); - setRightMotor((int)rightMotorValue); - break; +// case Qt::Key_Up: +// leftMotorValue = 1; +// rightMotorValue = 1; +// bothMotorsCheckBox->setChecked(true); +// leftMotorSlider->setValue((int)leftMotorValue); +// setLeftMotor((int)leftMotorValue); +// break; +// case Qt::Key_Down: +// leftMotorValue = -1; +// rightMotorValue = -1; +// bothMotorsCheckBox->setChecked(true); +// leftMotorSlider->setValue((int)leftMotorValue); +// setLeftMotor((int)leftMotorValue); +// break; +// case Qt::Key_Left: +// leftMotorValue = -1; +// rightMotorValue = 1; +// leftMotorSlider->setValue((int)leftMotorValue); +// rightMotorSlider->setValue((int)rightMotorValue); +// setLeftMotor((int)leftMotorValue); +// setRightMotor((int)leftMotorValue); +// break; +// case Qt::Key_Right: +// leftMotorValue = 1; +// rightMotorValue = -1; +// leftMotorSlider->setValue((int)leftMotorValue); +// rightMotorSlider->setValue((int)rightMotorValue); +// setLeftMotor((int)leftMotorValue); +// setRightMotor((int)rightMotorValue); +// break; default: event->ignore(); break; @@ -700,16 +745,16 @@ void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event) } switch (event->key()) { - case Qt::Key_Up: - case Qt::Key_Down: - case Qt::Key_Left: - case Qt::Key_Right: - leftMotorValue = 0; - rightMotorValue = 0; - bothMotorsCheckBox->setChecked(false); - leftMotorSlider->setValue((int)leftMotorValue); - rightMotorSlider->setValue((int)rightMotorValue); - break; +// case Qt::Key_Up: +// case Qt::Key_Down: +// case Qt::Key_Left: +// case Qt::Key_Right: +// leftMotorValue = 0; +// rightMotorValue = 0; +// bothMotorsCheckBox->setChecked(false); +// leftMotorSlider->setValue((int)leftMotorValue); +// rightMotorSlider->setValue((int)rightMotorValue); +// break; default: event->ignore(); break; @@ -730,7 +775,8 @@ void RobomonAtlantis::createOrte() int rv; orte.strength = 11; - + + memset(&orte, 0, sizeof(orte)); rv = robottype_roboorte_init(&orte); if (rv) { printf("RobomonAtlantis: Unable to initialize ORTE\n"); @@ -741,23 +787,30 @@ void RobomonAtlantis::createOrte() robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL); robottype_publisher_robot_cmd_create(&orte, NULL, &orte); + robottype_publisher_robot_switches_create(&orte, dummy_publisher_callback, &orte); /* subscribers */ - robottype_subscriber_pwr_voltage_create(&orte, + robottype_subscriber_pwr_voltage_create(&orte, receivePowerVoltageCallBack, this); - robottype_subscriber_motion_status_create(&orte, + robottype_subscriber_motion_status_create(&orte, receiveMotionStatusCallBack, this); - robottype_subscriber_ref_pos_create(&orte, + robottype_subscriber_ref_pos_create(&orte, receiveActualPositionCallBack, this); - robottype_subscriber_est_pos_uzv_create(&orte, - generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_UZV)); - robottype_subscriber_est_pos_odo_create(&orte, + robottype_subscriber_est_pos_odo_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO)); - robottype_subscriber_fsm_main_create(&orte, + robottype_subscriber_est_pos_indep_odo_create(&orte, + generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_INDEP_ODO)); + robottype_subscriber_est_pos_best_create(&orte, + generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST)); + robottype_subscriber_hokuyo_scan_create(&orte, + generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN)); + robottype_subscriber_jaws_cmd_create(&orte, + generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD)); + robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, this); - robottype_subscriber_fsm_motion_create(&orte, + robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, this); - robottype_subscriber_fsm_act_create(&orte, + robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, this); /* motors */ @@ -775,15 +828,11 @@ void RobomonAtlantis::createOrte() act_init(&orte); /* set actions to do when we receive data from orte */ - connect(this, SIGNAL(motionStatusReceivedSignal()), + connect(this, SIGNAL(motionStatusReceivedSignal()), this, SLOT(motionStatusReceived())); - connect(this, SIGNAL(actualPositionReceivedSignal()), + connect(this, SIGNAL(actualPositionReceivedSignal()), this, SLOT(actualPositionReceived())); - connect(this, SIGNAL(estimatedPositionReceivedSignal()), - this, SLOT(estimatedPositionReceived())); - connect(this, SIGNAL(diReceivedSignal()), - this, SLOT(diReceived())); - connect(this, SIGNAL(powerVoltageReceivedSignal()), + connect(this, SIGNAL(powerVoltageReceivedSignal()), this, SLOT(powerVoltageReceived())); } @@ -799,24 +848,11 @@ void RobomonAtlantis::actualPositionReceived() actPosPhi->setText(QString("%1(%2)") .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0) .arg(orte.ref_pos.phi, 0, 'f', 1)); - robotActPos->moveRobot(orte.ref_pos.x, + robotRefPos->moveRobot(orte.ref_pos.x, orte.ref_pos.y, orte.ref_pos.phi); trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y)); } -void RobomonAtlantis::estimatedPositionReceived() -{ - estPosX->setText(QString("%1").arg(orte.est_pos_uzv.x, 0, 'f', 3)); - estPosY->setText(QString("%1").arg(orte.est_pos_uzv.y, 0, 'f', 3)); - estPosPhi->setText(QString("%1(%2)") - .arg(DEGREES(orte.est_pos_uzv.phi), 0, 'f', 0) - .arg(orte.est_pos_uzv.phi, 0, 'f', 1)); - robotEstPosUzv->moveRobot(orte.est_pos_uzv.x, - orte.est_pos_uzv.y, orte.est_pos_uzv.phi); - trailUzvPos->addPoint(QPointF(orte.est_pos_uzv.x, - orte.est_pos_uzv.y)); -} - void RobomonAtlantis::powerVoltageReceived() { voltage33LineEdit->setText(QString("%1").arg( @@ -842,7 +878,7 @@ void RobomonAtlantis::openSharedMemory() return; sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT; - + /* Get segment identificator in a read only mode */ segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR); if(segmentId == -1) { @@ -850,10 +886,10 @@ void RobomonAtlantis::openSharedMemory() "Unable to open shared memory segment!"); return; } - + /* Init Shmap */ ShmapInit(0); - + /* Attach the shared memory segment */ //map = (_Map*)shmat (segmentId, (void*) 0, 0); @@ -868,7 +904,7 @@ double RobomonAtlantis::distanceToWallHokuyo(int beamnum) struct map *map = ShmapIsMapInit(); if (!map) return min_distance; - + // Simulate obstacles for(j=0;jflags & MAP_FLAG_SIMULATED_WALL) { // WALL ShmapCell2Point(i, j, &wall.x, &wall.y); - + distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M); if (distance +M_PI) angle -= 2.0*M_PI; + if (angle < -M_PI) angle += 2.0*M_PI; + angle = fabs(angle); + + double k = tan(sensor_a); + double r = diameter / 2.0; + + double A = 1 + k*k; + double B = 2 * (sensor.y*k - center.x - k*k*sensor.x - center.y*k); + double C = center.x*center.x + center.y*center.y + + k*k*sensor.x*sensor.x - 2*sensor.y*k*sensor.x + + sensor.y*sensor.y + 2*k*sensor.x*center.y - + 2*sensor.y*center.y - r*r; + + double D = B*B - 4*A*C; + + if (D > 0) { + Point ob1, ob2; + + ob1.x = (-B + sqrt(D)) / (2*A); + ob2.x = (-B - sqrt(D)) / (2*A); + ob1.y = k * (ob1.x - sensor.x) + sensor.y; + ob2.y = k * (ob2.x - sensor.x) + sensor.y; + + double distance1 = sensor.distanceTo(ob1); + double distance2 = sensor.distanceTo(ob2); + distance = (distance1 < distance2) ? distance1 : distance2; + } else if (D == 0) { + Point ob; + ob.x = -B / (2*A); + ob.y = k * (ob.x - sensor.x) + sensor.y; + distance = sensor.distanceTo(ob); + } + distance = distance + (drand48()-0.5)*3.0e-2; + if (D < 0 || angle > atan(r / distance)) + distance = sensorRange; + if (distance > sensorRange) + distance = sensorRange; + + return distance; +} + +/** * Calculation for Hokuyo simulation. Calculates distance that would * be returned by Hokuyo sensors, if there is only one obstacle (as * specified by parameters). @@ -894,39 +993,39 @@ double RobomonAtlantis::distanceToWallHokuyo(int beamnum) * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT] * @param obstacle Position of the obstacle (x, y in meters). * @param obstacleSize Size (diameter) of the obstacle in meters. - * + * * @return Distance measured by sensors in meters. - */ + */ double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize) -{ - struct robot_pos_type e = orte.est_pos_uzv; +{ + struct robot_pos_type e = orte.est_pos_best; double sensor_a; struct sharp_pos s; s.x = HOKUYO_CENTER_OFFSET_M; s.y = 0.0; - s.ang = HOKUYO_CLUSTER_TO_RAD(beamnum); + s.ang = HOKUYO_INDEX_TO_RAD(beamnum); Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi), e.y + s.x*sin(e.phi) + s.y*cos(e.phi)); sensor_a = e.phi + s.ang; - + const double sensorRange = 4.0; /*[meters]*/ - + double distance, angle; - + angle = sensor.angleTo(obstacle) - sensor_a; angle = fmod(angle, 2.0*M_PI); if (angle > +M_PI) angle -= 2.0*M_PI; if (angle < -M_PI) angle += 2.0*M_PI; angle = fabs(angle); - distance = sensor.distanceTo(obstacle)-0.11; - if (angle < atan(obstacleSize/2.0 / (distance+0.001))) { + distance = sensor.distanceTo(obstacle) - obstacleSize/2.0; + if (angle < atan(obstacleSize/2.0 / distance)) { // We can see the obstackle from here. if (angle < M_PI/2.0) { distance = distance/cos(angle); } - if (distance > sensorRange) + if (distance > sensorRange) distance = sensorRange; } else { distance = sensorRange; @@ -937,20 +1036,33 @@ double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, do void RobomonAtlantis::sendStart(int plug) { - orte.robot_cmd.start = plug ? 0 : 1; + orte.robot_cmd.start_condition = plug ? 0 : 1; ORTEPublicationSend(orte.publication_robot_cmd); } +void RobomonAtlantis::setTeamColor(int plug) +{ + orte.robot_switches.team_color = plug ? 1 : 0; + ORTEPublicationSend(orte.publication_robot_switches); +} + void RobomonAtlantis::resetTrails() { trailRefPos->reset(); - trailUzvPos->reset(); + trailEstPosBest->reset(); + trailPosIndepOdo->reset(); trailOdoPos->reset(); } void RobomonAtlantis::showTrails(bool show) { - trailRefPos->setVisible(show); - trailUzvPos->setVisible(show); - trailOdoPos->setVisible(show); + trailRefPos->setVisible(show && robotRefPos->isVisible()); + trailEstPosBest->setVisible(show && robotEstPosBest->isVisible()); + trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible()); + trailOdoPos->setVisible(show && robotEstPosOdo->isVisible()); +} + +void RobomonAtlantis::showShapeDetect(bool show) +{ + hokuyoScan->showShapeDetect = show; }