#include <stdio.h>
#include <unistd.h>
#include <math.h>
+#include <errno.h>
#include <orte.h>
#include <path_planner.h>
#include "robomon_orte.h"
#include "RobomonAtlantis.h"
#include "playgroundview.h"
+#include "trail.h"
#include <QCoreApplication>
#include <QEvent>
createOrte();
createRobots();
createActions();
+ createMap();
+
+// connect(vidle, SIGNAL(valueChanged(int)),
+// robotEstPosBest, SLOT(setVidle(int)));
setFocusPolicy(Qt::StrongFocus);
sharedMemoryOpened = false;
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();
- createDIOGroupBox();
createPowerGroupBox();
- createSensorsGroupBox();
- vlayout->addWidget(positionGroupBox);
- vlayout->addWidget(miscGroupBox);
- vlayout->addWidget(fsmGroupBox);
- layout->addLayout(vlayout, 0, 0);
- layout->addWidget(actuatorsGroupBox, 0, 1);
-// layout->addWidget(dioGroupBox, 0, 2);
-
- rightLayout->addLayout(layout);
+ rightLayout->addWidget(positionGroupBox);
+ rightLayout->addWidget(miscGroupBox);
+ rightLayout->addWidget(fsmGroupBox);
rightLayout->addWidget(powerGroupBox);
- rightLayout->addWidget(sensorsGroupBox);
+ rightLayout->addWidget(actuatorsGroupBox);
}
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);
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();
estPosX = new QLineEdit();
estPosY = new QLineEdit();
estPosPhi = new QLineEdit();
-
+
actPosX->setReadOnly(true);
actPosY->setReadOnly(true);
actPosPhi->setReadOnly(true);
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);
void RobomonAtlantis::createMiscGroupBox()
{
miscGroupBox = new QGroupBox(tr("Miscellaneous"));
+ miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
QGridLayout *layout = new QGridLayout();
- showMapPushButton = new QCheckBox(tr("Show &map"));
- showMapPushButton->setShortcut(tr("m"));
-
- layout->addWidget(showMapPushButton, 0, 0);
-
obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
obstacleSimulationCheckBox->setShortcut(tr("o"));
layout->addWidget(obstacleSimulationCheckBox);
+ startPlug = new QCheckBox("&Start plug");
+ layout->addWidget(startPlug);
+
+ colorChoser = new QCheckBox("&Team color");
+ layout->addWidget(colorChoser);
+
+ strategyButton= new QPushButton(tr("Strategy"));
+ layout->addWidget(strategyButton);
+
miscGroupBox->setLayout(layout);
}
void RobomonAtlantis::createFSMGroupBox()
{
fsmGroupBox = new QGroupBox(tr("FSM"));
+ fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
QGridLayout *layout = new QGridLayout();
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);
void RobomonAtlantis::createDebugGroupBox()
{
debugGroupBox = new QGroupBox(tr("Debug window"));
+ debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
QHBoxLayout *layout = new QHBoxLayout();
debugWindow = new QTextEdit();
void RobomonAtlantis::createActuatorsGroupBox()
{
actuatorsGroupBox = new QGroupBox(tr("Actuators"));
+ actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
QHBoxLayout *layout = new QHBoxLayout();
+// vidle = new QDial();
+
+// vidle->setMinimum(VIDLE_VYSIP);
+// vidle->setMaximum((VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP);
+// vidle->setEnabled(true);
- createMotorsGroupBox();
- createPickerGroupBox();
+ //createMotorsGroupBox();
layout->setAlignment(Qt::AlignLeft);
- layout->addWidget(enginesGroupBox);
- layout->addWidget(pickerGroupBox);
+// layout->addWidget(vidle);
+ //layout->addWidget(enginesGroupBox);
actuatorsGroupBox->setLayout(layout);
}
-void RobomonAtlantis::createDIOGroupBox()
-{
- dioGroupBox = new QGroupBox(tr("DIO"));
- QGridLayout *layout = new QGridLayout();
- QFont font;
-
- font.setPointSize(5);
- dioGroupBox->setFont(font);
-
- for (int i=0; i<8; i++) {
- diCheckBox[i] = new QCheckBox(QString("DI%1").arg(i));
- doCheckBox[i] = new QCheckBox(QString("D0%1").arg(i));
- layout->addWidget(diCheckBox[i], i, 0);
- layout->addWidget(doCheckBox[i], i+8, 0);
- }
-
- dioGroupBox->setMaximumWidth(70);
- dioGroupBox->setLayout(layout);
-}
-
void RobomonAtlantis::createPowerGroupBox()
{
powerGroupBox = new QGroupBox(tr("Power management"));
+ powerGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
QGridLayout *layout = new QGridLayout();
voltage33CheckBox = new QCheckBox(tr("&3.3V"));
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();
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::createSensorsGroupBox()
+void RobomonAtlantis::createRobots()
{
- sensorsGroupBox = new QGroupBox(tr("Sensors"));
- QHBoxLayout *layout = new QHBoxLayout();
+ robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
+ robotRefPos->setZValue(11);
+ trailRefPos = new Trail(QPen(Qt::darkBlue));
+ trailRefPos->setZValue(11);
- layout->addWidget(MiscGui::createLabel("Puck Sharp"));
- sharpPuck = new QProgressBar();
- sharpPuck->setFormat("%v mm");
- sharpPuck->setTextVisible(true);
- sharpPuck->setMaximum(300);
- layout->addWidget(sharpPuck);
+ robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray));
+ robotEstPosBest->setZValue(10);
+ trailEstPosBest = new Trail(QPen());
+ trailEstPosBest->setZValue(10);
- sensorsGroupBox->setLayout(layout);
-}
+ robotEstPosOdo = new Robot("Mot", QPen(Qt::white), QBrush(Qt::darkRed));
+ robotEstPosOdo->setZValue(10);
+ trailOdoPos = new Trail(QPen(Qt::red));
+ trailOdoPos->setZValue(10);
-void RobomonAtlantis::createMotorsGroupBox()
-{
- enginesGroupBox = new QGroupBox(tr("Motors"));
- QVBoxLayout *layout = new QVBoxLayout();
- QHBoxLayout *layout1 = new QHBoxLayout();
- QHBoxLayout *layout2 = new QHBoxLayout();
-
- leftMotorSlider = new QSlider(Qt::Vertical);
- rightMotorSlider = new QSlider(Qt::Vertical);
- bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
- stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
-
- leftMotorSlider->setMinimum(-100);
- leftMotorSlider->setMaximum(100);
- leftMotorSlider->setTracking(false);
- leftMotorSlider->setTickPosition(QSlider::TicksLeft);
-
- rightMotorSlider->setMinimum(-100);
- rightMotorSlider->setMaximum(100);
- rightMotorSlider->setTracking(false);
- rightMotorSlider->setTickPosition(QSlider::TicksRight);
-
- stopMotorsPushButton->setMaximumWidth(90);
-
- layout1->addWidget(leftMotorSlider);
- layout1->addWidget(MiscGui::createLabel("0"));
- layout1->addWidget(rightMotorSlider);
-
- layout2->addWidget(bothMotorsCheckBox);
-
- layout->addWidget(MiscGui::createLabel("100"));
- layout->addLayout(layout1);
- layout->addWidget(MiscGui::createLabel("-100"));
- layout->addLayout(layout2);
- layout->addWidget(stopMotorsPushButton);
- enginesGroupBox->setLayout(layout);
-}
+ robotEstPosIndepOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkGreen));
+ robotEstPosIndepOdo->setZValue(10);
+ trailPosIndepOdo = new Trail(QPen(Qt::green));
+ trailPosIndepOdo->setZValue(10);
+
+ playgroundScene->addItem(robotRefPos);
+ playgroundScene->addItem(robotEstPosBest);
+ playgroundScene->addItem(robotEstPosIndepOdo);
+ playgroundScene->addItem(robotEstPosOdo);
+
+ showTrails(false);
+
+ playgroundScene->addItem(trailRefPos);
+ playgroundScene->addItem(trailPosIndepOdo);
+ playgroundScene->addItem(trailOdoPos);
+
+ hokuyoScan = new HokuyoScan();
+ hokuyoScan->setZValue(10);
+ playgroundScene->addItem(hokuyoScan);
-void RobomonAtlantis::createPickerGroupBox()
-{
- pickerGroupBox = new QGroupBox(tr("Picker"));
- QVBoxLayout *layout = new QVBoxLayout();
-
- leftBeltCheckBox = new QCheckBox(tr("L Belt"));
- rightBeltCheckBox = new QCheckBox(tr("R Belt"));
- leftChelaCheckBox = new QCheckBox(tr("L Chela"));
- rightChelaCheckBox = new QCheckBox(tr("R Chela"));
- leftBeltDial = new QDial();
- rightBeltDial = new QDial();
- leftChelaDial = new QDial();
- rightChelaDial = new QDial();
- pickPushButton = new QPushButton(tr("Pick!"));
-
- leftBeltDial->setMinimum(0);
- leftBeltDial->setMaximum(200);
- leftBeltDial->setValue(100);
-
- rightBeltDial->setMinimum(0);
- rightBeltDial->setMaximum(200);
- rightBeltDial->setValue(100);
-
- leftChelaDial->setMinimum(0);
- leftChelaDial->setMaximum(255);
- leftChelaDial->setValue(127);
-
- rightChelaDial->setMinimum(0);
- rightChelaDial->setMaximum(255);
- rightChelaDial->setValue(127);
-
- layout->addWidget(leftChelaDial);
- layout->addWidget(leftChelaCheckBox);
- layout->addWidget(rightChelaDial);
- layout->addWidget(rightChelaCheckBox);
- layout->addWidget(leftBeltDial);
- layout->addWidget(leftBeltCheckBox);
- layout->addWidget(rightBeltDial);
- layout->addWidget(rightBeltCheckBox);
- layout->addWidget(pickPushButton);
-
- pickerGroupBox->setLayout(layout);
}
-void RobomonAtlantis::createRobots()
+void RobomonAtlantis::createMap()
{
- robotActPos = new Robot(QPen(), QBrush(Qt::darkGray));
- robotActPos->setZValue(10);
- robotEstPos = new Robot(QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
- robotEstPos->setZValue(11);
+ mapImage = new Map();
+ mapImage->setZValue(5);
+ mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
- playgroundScene->addItem(robotActPos);
- playgroundScene->addItem(robotEstPos);
+
+ playgroundScene->addItem(mapImage);
}
/**********************************************************************
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()));
-
- /* DIO */
- for (int i=0; i<8; i++)
- connect(doCheckBox[0], SIGNAL(stateChanged(int)),
- this, SLOT(setDO(int)));
-
- /* path planning map */
- connect(showMapPushButton, SIGNAL(clicked()),
- this, SLOT(showMap()));
+// 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(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;
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)));
+}
- /* Actuators */
- connect(pickPushButton, SIGNAL(clicked()), this, SLOT(pick()));
- connect(leftBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
- connect(rightBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
- connect(leftChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
- connect(rightChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
- connect(leftBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
- connect(rightBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
- connect(leftChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
- connect(rightChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
+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)
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::stopMotors()
-{
- leftMotorSlider->setValue(0);
- rightMotorSlider->setValue(0);
-}
+// }
+// void RobomonAtlantis::setRightMotor(int value)
+// {
+// short int leftMotor;
+// short int rightMotor;
-void RobomonAtlantis::setDO(int state)
-{
- Q_UNUSED(state);
- /* FIXME: digital output control comes here */
-}
+// if(bothMotorsCheckBox->isChecked())
+// leftMotorSlider->setValue(value);
-void RobomonAtlantis::pick()
-{
- // TODO: send signal to the fsmact
-}
+// leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
+// rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
-void RobomonAtlantis::setBelts(int value)
-{
- Q_UNUSED(value);
- unsigned char leftBelt;
- unsigned char rightBelt;
+// orte.motion_speed.left = leftMotor;
+// orte.motion_speed.right = rightMotor;
- leftBelt = (unsigned char)leftBeltDial->value();
- rightBelt = (unsigned char)rightBeltDial->value();
+// }
- act_belts(leftBelt, rightBelt);
-}
+// void RobomonAtlantis::stopMotors()
+// {
+// leftMotorSlider->setValue(0);
+// rightMotorSlider->setValue(0);
+// }
-void RobomonAtlantis::setChelae(int value)
+void RobomonAtlantis::useOpenGL(bool use)
{
- Q_UNUSED(value);
- unsigned char leftChela;
- unsigned char rightChela;
-
- leftChela = (unsigned char)leftChelaDial->value();
- rightChela = (unsigned char)rightChelaDial->value();
-
- act_chelae(leftChela, rightChela);
+ playgroundSceneView->useOpenGL(&use);
}
-void RobomonAtlantis::showMap()
+void RobomonAtlantis::showMap(bool show)
{
openSharedMemory();
if (sharedMemoryOpened == false)
return;
- mapTimer = new QTimer(this);
- connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
- mapTimer->start(200);
-
- disconnect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
- connect(showMapPushButton, SIGNAL(clicked()),
- this, SLOT(showPlayground()));
- playgroundScene->showMap(true);
-}
-
-void RobomonAtlantis::showPlayground()
-{
- mapTimer->stop();
- disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
-
- disconnect(showMapPushButton, SIGNAL(clicked()),
- this, SLOT(showPlayground()));
- connect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
- playgroundScene->showMap(false);
+ if (show) {
+ mapTimer = new QTimer(this);
+ connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
+ mapTimer->start(200);
+ } else {
+ if(mapTimer != NULL) {
+ mapTimer->stop();
+ disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
+ }
+ }
+ mapImage->setVisible(show);
}
void RobomonAtlantis::paintMap()
{
using namespace Qt;
- int x, y;
struct map *map = ShmapIsMapInit();
if (!map) return;
-
- for(int i=0; i < MAP_WIDTH; i++) {
- for(int j=0; j<MAP_HEIGHT; j++) {
+
+ for(int i = 0; i < MAP_WIDTH; i++) {
+ for(int j = 0; j < MAP_HEIGHT; j++) {
QColor color;
-
+
struct map_cell *cell = &map->cells[j][i];
color = lightGray;
- if (cell->flags & MAP_FLAG_WALL)
+
+ if ((cell->flags & MAP_FLAG_WALL) &&
+ (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
color = darkYellow;
if (cell->flags & MAP_FLAG_IGNORE_OBST)
color = darkGreen;
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;
}
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;
simulationEnabled = state;
}
-/*!
+/*!
\fn RobomonAtlantis::setObstacleSimulation(int state)
*/
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;
}
void RobomonAtlantis::simulateObstaclesHokuyo()
{
double distance, wall_distance;
- int i;
+ unsigned int i;
uint16_t *hokuyo = orte.hokuyo_scan.data;
-
- for (i=0; i<HOKUYO_CLUSTER_CNT; i++) {
+
+ for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
wall_distance = distanceToWallHokuyo(i);
- distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
- if (wall_distance < distance)
+
+ distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
+ if (wall_distance < distance)
distance = wall_distance;
hokuyo[i] = distance*1000;
}
ORTEPublicationSend(orte.publication_hokuyo_scan);
-
+
}
void RobomonAtlantis::changeObstacle(QPointF position)
simulatedObstacle.x = position.x();
simulatedObstacle.y = position.y();
simulateObstaclesHokuyo();
-}
+}
/**********************************************************************
* EVENTS
case QEVENT(QEV_MOTION_STATUS):
emit motionStatusReceivedSignal();
break;
- case QEVENT(QEV_ACTUAL_POSITION):
- emit actualPositionReceivedSignal();
+ case QEVENT(QEV_HOKUYO_SCAN):
+ hokuyoScan->newScan(&orte.hokuyo_scan);
break;
- case QEVENT(QEV_ESTIMATED_POSITION):
- emit estimatedPositionReceivedSignal();
+ 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_DI):
- emit diReceivedSignal();
+ 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_ACCELEROMETER):
- emit accelerometerReceivedSignal();
+ case QEVENT(QEV_ESTIMATED_POSITION_ODO):
+ 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,
+ orte.est_pos_odo.y));
break;
- case QEVENT(QEV_ACCUMULATOR):
- emit accumulatorReceivedSignal();
+ 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();
case QEVENT(QEV_FSM_MOTION):
fsm_motion_state->setText(orte.fsm_motion.state_name);
break;
- case QEVENT(QEV_PUCK_DISTANCE):
- sharpPuck->setValue(orte.puck_distance.distance*1000);
- break;
default:
if (event->type() == QEvent::Close)
closeEvent((QCloseEvent *)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();
}
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;
}
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;
{
int rv;
- orte.strength = 11;
-
+ memset(&orte, 0, sizeof(orte));
rv = robottype_roboorte_init(&orte);
if (rv) {
printf("RobomonAtlantis: Unable to initialize ORTE\n");
robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
- robottype_publisher_lift_create(&orte, NULL, &orte);
- robottype_publisher_pusher_create(&orte, NULL, &orte);
- robottype_publisher_chelae_create(&orte, NULL, &orte);
- robottype_publisher_belts_create(&orte, NULL, &orte);
- robottype_publisher_holder_create(&orte, NULL, &orte);
+ 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_create(&orte,
- receiveEstimatedPositionCallBack, this);
- robottype_subscriber_fsm_main_create(&orte,
+ robottype_subscriber_est_pos_odo_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
+ 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);
- robottype_subscriber_puck_distance_create(&orte, rcv_puck_distance_cb, this);
- //robottype_subscriber_actuator_status_create(orte, receiveActuatorStatusCallBack, this);
-
- /*createDISubscriber(this, &orteData);*/
- /*createAccelerometerSubscriber(this, &orteData);*/
- /*createAccumulatorSubscriber(this, &orteData);*/
/* motors */
orte.motion_speed.left = 0;
orte.motion_speed.right = 0;
+
/* power management */
orte.pwr_ctrl.voltage33 = true;
orte.pwr_ctrl.voltage50 = true;
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(accelerometerReceivedSignal()),
- this, SLOT(accelerometerReceived()));
- connect(this, SIGNAL(accumulatorReceivedSignal()),
- this, SLOT(accumulatorReceived()));
- connect(this, SIGNAL(powerVoltageReceivedSignal()),
+ connect(this, SIGNAL(powerVoltageReceivedSignal()),
this, SLOT(powerVoltageReceived()));
}
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);
-}
-
-void RobomonAtlantis::estimatedPositionReceived()
-{
- estPosX->setText(QString("%1").arg(orte.est_pos.x, 0, 'f', 3));
- estPosY->setText(QString("%1").arg(orte.est_pos.y, 0, 'f', 3));
- estPosPhi->setText(QString("%1(%2)")
- .arg(DEGREES(orte.est_pos.phi), 0, 'f', 0)
- .arg(orte.est_pos.phi, 0, 'f', 1));
- robotEstPos->moveRobot(orte.est_pos.x,
- orte.est_pos.y, orte.est_pos.phi);
-}
-
-void RobomonAtlantis::diReceived()
-{
- WDBG("ORTE received: DI");
-}
-
-void RobomonAtlantis::accelerometerReceived()
-{
- WDBG("ORTE received: accelerometer");
-}
-
-void RobomonAtlantis::accumulatorReceived()
-{
- WDBG("ORTE received: accumulator");
+ trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
}
void RobomonAtlantis::powerVoltageReceived()
if (sharedMemoryOpened)
return;
- sharedSegmentSize = sizeof(char) * MAP_WIDTH * MAP_HEIGHT;
-
+ 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) {
"Unable to open shared memory segment!");
return;
}
-
+
/* Init Shmap */
ShmapInit(0);
-
+
/* Attach the shared memory segment */
//map = (_Map*)shmat (segmentId, (void*) 0, 0);
struct map *map = ShmapIsMapInit();
if (!map) return min_distance;
-
+
// Simulate obstacles
for(j=0;j<MAP_HEIGHT;j++) {
for (i=0;i<MAP_WIDTH;i++) {
if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
// WALL
ShmapCell2Point(i, j, &wall.x, &wall.y);
-
+
distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
if (distance<min_distance) min_distance = distance;
}
return min_distance;
}
-/**
+double RobomonAtlantis::distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter)
+{
+ 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_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 = sensorRange;
+ double angle;
+
+ angle = sensor.angleTo(center) - 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);
+
+ 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).
* @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 est_pos_type e = orte.est_pos;
+{
+ 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;
return distance;
}
+
+void RobomonAtlantis::sendStart(int plug)
+{
+ 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();
+ trailEstPosBest->reset();
+ trailPosIndepOdo->reset();
+ trailOdoPos->reset();
+}
+
+void RobomonAtlantis::showTrails(bool 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;
+}
projects / eurobot / public.git / blobdiff