#include <trgen.h>
#include <map.h>
#include <robomath.h>
-#include <hokuyo.h>
+#include <lidar_params.h>
#include <actuators.h>
#include "PlaygroundScene.h"
#include "MiscGui.h"
#include <QDebug>
#include <QMessageBox>
-RobomonAtlantis::RobomonAtlantis(QWidget *parent)
- : QWidget(parent)
+RobomonAtlantis::RobomonAtlantis(QStatusBar *_statusBar)
+ : QWidget(0), statusBar(_statusBar)
{
QFont font;
font.setPointSize(7);
createActions();
createMap();
-// connect(vidle, SIGNAL(valueChanged(int)),
-// robotEstPosBest, SLOT(setVidle(int)));
-
setFocusPolicy(Qt::StrongFocus);
sharedMemoryOpened = false;
WDBG("Youuuhouuuu!!");
rightLayout = new QVBoxLayout();
createPositionGroupBox();
+ createObstSimGroupBox();
createMiscGroupBox();
createFSMGroupBox();
createActuatorsGroupBox();
createPowerGroupBox();
rightLayout->addWidget(positionGroupBox);
+ rightLayout->addWidget(obstSimGroupBox);
rightLayout->addWidget(miscGroupBox);
rightLayout->addWidget(fsmGroupBox);
- rightLayout->addWidget(powerGroupBox);
rightLayout->addWidget(actuatorsGroupBox);
}
playgroundScene = new PlaygroundScene();
playgroundSceneView = new PlaygroundView(playgroundScene);
- //playgroundSceneView->setMinimumWidth(630);
- //playgroundSceneView->setMinimumHeight(445);
- playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
+ playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
playgroundSceneView->setMouseTracking(true);
void RobomonAtlantis::createPositionGroupBox()
{
positionGroupBox = new QGroupBox(tr("Position state"));
- positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
+ positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
QGridLayout *layout = new QGridLayout();
actPosX = new QLineEdit();
miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
QGridLayout *layout = new QGridLayout();
- 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::createObstSimGroupBox()
+{
+ obstSimGroupBox = new QGroupBox(tr("Obstacle simulation"));
+ obstSimGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
+ QGridLayout *layout = new QGridLayout();
+
+ hokuyoSimCheckBox = new QCheckBox(tr("&Hokuyo lidar simulation"));
+ hokuyoSimCheckBox->setShortcut(tr("h"));
+ layout->addWidget(hokuyoSimCheckBox);
+
+ sick331SimCheckBox = new QCheckBox(tr("Sick Tim &331 lidar simulation"));
+ sick331SimCheckBox->setShortcut(tr("3"));
+ layout->addWidget(sick331SimCheckBox);
+
+ sick551SimCheckBox = new QCheckBox(tr("Sick Tim &551 lidar simulation"));
+ sick551SimCheckBox->setShortcut(tr("5"));
+ layout->addWidget(sick551SimCheckBox);
+
+ obstSimGroupBox->setLayout(layout);
+}
+
void RobomonAtlantis::createFSMGroupBox()
{
fsmGroupBox = new QGroupBox(tr("FSM"));
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();
layout->setAlignment(Qt::AlignLeft);
-// layout->addWidget(vidle);
- //layout->addWidget(enginesGroupBox);
actuatorsGroupBox->setLayout(layout);
}
playgroundScene->addItem(trailPosIndepOdo);
playgroundScene->addItem(trailOdoPos);
- hokuyoScan = new HokuyoScan();
+ hokuyoScan = new LidarScan(hokuyo_params);
hokuyoScan->setZValue(10);
playgroundScene->addItem(hokuyoScan);
+ sickScan = new LidarScan(sick_params);
+ sickScan->setZValue(10);
+ playgroundScene->addItem(sickScan);
+
+ sick551Scan = new LidarScan(sick551_params);
+ sick551Scan->setZValue(10);
+ playgroundScene->addItem(sick551Scan);
}
void RobomonAtlantis::createMap()
mapImage->setZValue(5);
mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
-
playgroundScene->addItem(mapImage);
}
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(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;
- connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
- this, SLOT(setSimulation(int)));
- connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
- this, SLOT(setObstacleSimulation(int)));
- connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
- playgroundScene, SLOT(showObstacle(int)));
+ hokuyoSimEnabled = 0;
+ sickSimEnabled = 0;
+ sick551SimEnabled = 0;
+
+ connect(hokuyoSimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setHokuyoSimulation(int)));
+ connect(hokuyoSimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setHokuyoObstacleSimulation(int)));
+
+ connect(sick331SimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setSick331Simulation(int)));
+ connect(sick331SimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setSick331ObstacleSimulation(int)));
+
+ connect(sick551SimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setSick551Simulation(int)));
+ connect(sick551SimCheckBox, SIGNAL(stateChanged(int)),
+ this, SLOT(setSick551ObstacleSimulation(int)));
+
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)
orte.pwr_ctrl.voltage80 = false;
}
-// void RobomonAtlantis::setLeftMotor(int value)
-// {
-// short int leftMotor;
-// short int rightMotor;
-
-// if(bothMotorsCheckBox->isChecked())
-// rightMotorSlider->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::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::useOpenGL(bool use)
{
playgroundSceneView->useOpenGL(&use);
void RobomonAtlantis::paintMap()
{
- using namespace Qt;
- struct map *map = ShmapIsMapInit();
+ using namespace Qt;
+ struct map *map = ShmapIsMapInit();
- if (!map) return;
+ if (!map) return;
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) &&
- (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
- color = darkYellow;
- if (cell->flags & MAP_FLAG_IGNORE_OBST)
- color = darkGreen;
- if (cell->flags & MAP_FLAG_SIMULATED_WALL)
- color = yellow;
- if (cell->flags & MAP_FLAG_PATH)
- color = darkRed;
- if (cell->flags & MAP_FLAG_START)
- color = red;
- if (cell->flags & MAP_FLAG_GOAL)
- color = green;
+ QColor color;
+
+ struct map_cell *cell = &map->cells[j][i];
+ color = lightGray;
+
+ if (cell->flags & MAP_FLAG_WALL)
+ color = darkYellow;
+ if (cell->flags & MAP_FLAG_IGNORE_OBST)
+ color = darkGreen;
+ if (cell->flags & MAP_FLAG_SIMULATED_WALL)
+ color = yellow;
+ if (cell->flags & MAP_FLAG_PATH)
+ color = darkRed;
+ if (cell->flags & MAP_FLAG_START)
+ 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;
- QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
- c1.green() + (int)(f*(c2.green() - c1.green())),
- c1.blue() + (int)(f*(c2.blue() - c1.blue())));
- color = c;
- }
- if (cell->flags & MAP_FLAG_DET_OBST)
- color = cyan;
+ if (cell->detected_obstacle) {
+ QColor c1(color), c2(blue);
+ double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
+ QColor c(c1.red() + (int)(f*(c2.red() - c1.red())),
+ c1.green() + (int)(f*(c2.green() - c1.green())),
+ c1.blue() + (int)(f*(c2.blue() - c1.blue())));
+ color = c;
+ }
+ if (cell->flags & MAP_FLAG_DET_OBST)
+ color = cyan;
color.setAlpha(200);
- mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
- }
+ mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
+ }
}
}
-void RobomonAtlantis::setSimulation(int state)
+void RobomonAtlantis::setHokuyoSimulation(int state)
{
if(state) {
robottype_publisher_hokuyo_scan_create(&orte, NULL, this);
} else {
- if (!simulationEnabled)
+ if (!hokuyoSimEnabled)
return;
robottype_publisher_hokuyo_scan_destroy(&orte);
}
- simulationEnabled = state;
+ hokuyoSimEnabled = state;
}
-/*!
- \fn RobomonAtlantis::setObstacleSimulation(int state)
- */
-void RobomonAtlantis::setObstacleSimulation(int state)
+void RobomonAtlantis::setSick331Simulation(int state)
+{
+ if(state) {
+ robottype_publisher_sick_scan_create(&orte, NULL, this);
+ } else {
+ if (!sickSimEnabled)
+ return;
+ robottype_publisher_sick_scan_destroy(&orte);
+ }
+ sickSimEnabled = state;
+}
+
+void RobomonAtlantis::setSick551Simulation(int state)
+{
+ if(state) {
+ robottype_publisher_sick551_scan_create(&orte, NULL, this);
+ } else {
+ if (!sick551SimEnabled)
+ return;
+ robottype_publisher_sick551_scan_destroy(&orte);
+ }
+ sick551SimEnabled = state;
+}
+
+void RobomonAtlantis::setHokuyoObstacleSimulation(int state)
{
if (state) {
- /* TODO Maybe it is possible to attach only once to Shmap */
- ShmapInit(0);
- obstacleSimulationTimer = new QTimer(this);
- connect(obstacleSimulationTimer, SIGNAL(timeout()),
+ /* TODO Maybe it is possible to attach only once to Shmap */
+ ShmapInit(0);
+ obstacleSimulationTimerHokuyo = new QTimer(this);
+ connect(obstacleSimulationTimerHokuyo, SIGNAL(timeout()),
this, SLOT(simulateObstaclesHokuyo()));
- obstacleSimulationTimer->start(100);
- setMouseTracking(true);
+ obstacleSimulationTimerHokuyo->start(100);
+ setMouseTracking(true);
+ hokuyoScan->setVisible(true);
} else {
- if (obstacleSimulationTimer)
- delete obstacleSimulationTimer;
- //double distance = 0.8;
+ if (obstacleSimulationTimerHokuyo)
+ delete obstacleSimulationTimerHokuyo;
+ // Hide scans of lidars
+ hokuyoScan->setVisible(false);
}
}
+void RobomonAtlantis::setSick331ObstacleSimulation(int state)
+{
+ if (state) {
+ /* TODO Maybe it is possible to attach only once to Shmap */
+ ShmapInit(0);
+ obstacleSimulationTimerSick331 = new QTimer(this);
+ connect(obstacleSimulationTimerSick331, SIGNAL(timeout()),
+ this, SLOT(simulateObstaclesSick()));
+ obstacleSimulationTimerSick331->start(100);
+ setMouseTracking(true);
+ sickScan->setVisible(true);
+ } else {
+ if (obstacleSimulationTimerSick331)
+ delete obstacleSimulationTimerSick331;
+ // Hide scans of lidars
+ sickScan->setVisible(false);
+ }
+}
-void RobomonAtlantis::simulateObstaclesHokuyo()
+void RobomonAtlantis::setSick551ObstacleSimulation(int state)
+{
+ if (state) {
+ /* TODO Maybe it is possible to attach only once to Shmap */
+ ShmapInit(0);
+ obstacleSimulationTimerSick551 = new QTimer(this);
+ connect(obstacleSimulationTimerSick551, SIGNAL(timeout()),
+ this, SLOT(simulateObstaclesSick551()));
+ obstacleSimulationTimerSick551->start(100);
+ setMouseTracking(true);
+ sick551Scan->setVisible(true);
+ } else {
+ if (obstacleSimulationTimerSick551)
+ delete obstacleSimulationTimerSick551;
+ // Hide scans of lidars
+ sick551Scan->setVisible(false);
+ }
+}
+
+void RobomonAtlantis::simulateObstaclesLidar(const struct lidar_params lidar)
{
double distance, wall_distance;
unsigned int i;
- uint16_t *hokuyo = orte.hokuyo_scan.data;
-
- for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
- wall_distance = distanceToWallHokuyo(i);
+ unsigned int data_lenght = 0;
+ uint16_t *lidar_data = NULL;
+ switch (lidar.type) {
+ case HOKUYO:
+ lidar_data = orte.hokuyo_scan.data;
+ data_lenght = hokuyo_params.data_lenght;
+ break;
+ case SICK_TIM3XX:
+ lidar_data = orte.sick_scan.data;
+ data_lenght = sick_params.data_lenght;
+ break;
+ case SICK_TIM551:
+ lidar_data = orte.sick551_scan.data;
+ data_lenght = sick551_params.data_lenght;
+ break;
+ default:
+ return;
+ }
- distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
+ for (i = 0; i < data_lenght; i++) {
+ wall_distance = distanceToWallLidar(lidar, i);
+ distance = distanceToCircularObstacleLidar(lidar, i, simulatedObstacle, SIM_OBST_SIZE_M);
if (wall_distance < distance)
distance = wall_distance;
- hokuyo[i] = distance*1000;
+ lidar_data[i] = distance*1000;
+ }
+
+ switch (lidar.type) {
+ case HOKUYO:
+ orte.hokuyo_scan.data_lenght = hokuyo_params.data_lenght;
+ orte.hokuyo_scan.lidar_type = hokuyo_params.type;
+ ORTEPublicationSend(orte.publication_hokuyo_scan);
+ break;
+ case SICK_TIM3XX:
+ orte.sick_scan.data_lenght = sick_params.data_lenght;
+ orte.sick_scan.lidar_type = sick_params.type;
+ ORTEPublicationSend(orte.publication_sick_scan);
+ break;
+ case SICK_TIM551:
+ orte.sick551_scan.data_lenght = sick551_params.data_lenght;
+ orte.sick551_scan.lidar_type = sick551_params.type;
+ ORTEPublicationSend(orte.publication_sick551_scan);
+ break;
+ default:
+ return;
}
- ORTEPublicationSend(orte.publication_hokuyo_scan);
+}
+void RobomonAtlantis::simulateObstaclesHokuyo()
+{
+ simulateObstaclesLidar(hokuyo_params);
+}
+
+void RobomonAtlantis::simulateObstaclesSick()
+{
+ simulateObstaclesLidar(sick_params);
+}
+
+void RobomonAtlantis::simulateObstaclesSick551()
+{
+ simulateObstaclesLidar(sick551_params);
}
void RobomonAtlantis::changeObstacle(QPointF position)
{
- if (!simulationEnabled) {
- simulationEnabled = 1;
- obstacleSimulationCheckBox->setChecked(true);
+ if (!hokuyoSimEnabled && !sickSimEnabled && !sick551SimEnabled) {
+ hokuyoSimEnabled = 1;
+ sickSimEnabled = 1;
+ sick551SimEnabled = 1;
+ hokuyoSimCheckBox->setChecked(true);
+ sick331SimCheckBox->setChecked(true);
+ sick551SimCheckBox->setChecked(true);
}
simulatedObstacle.x = position.x();
simulatedObstacle.y = position.y();
- simulateObstaclesHokuyo();
}
/**********************************************************************
case QEVENT(QEV_MOTION_STATUS):
emit motionStatusReceivedSignal();
break;
+ case QEVENT(QEV_SICK_SCAN):
+ sickScan->newScan(&orte.sick_scan);
+ break;
+ case QEVENT(QEV_SICK551_SCAN):
+ sick551Scan->newScan(&orte.sick551_scan);
+ break;
case QEVENT(QEV_HOKUYO_SCAN):
hokuyoScan->newScan(&orte.hokuyo_scan);
break;
- case QEVENT(QEV_VIDLE_CMD):
- robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
- break;
case QEVENT(QEV_REFERENCE_POSITION):
emit actualPositionReceivedSignal();
break;
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));
+ orte.est_pos_indep_odo.y));
break;
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));
+ 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));
+ orte.est_pos_best.y));
hokuyoScan->setPosition(orte.est_pos_best.x,
orte.est_pos_best.y,
orte.est_pos_best.phi);
+ sickScan->setPosition(orte.est_pos_best.x,
+ orte.est_pos_best.y,
+ orte.est_pos_best.phi);
+ sick551Scan->setPosition(orte.est_pos_best.x,
+ orte.est_pos_best.y,
+ orte.est_pos_best.phi);
break;
case QEVENT(QEV_POWER_VOLTAGE):
emit powerVoltageReceivedSignal();
{
int rv;
- orte.strength = 11;
-
memset(&orte, 0, sizeof(orte));
rv = robottype_roboorte_init(&orte);
if (rv) {
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_sick_scan_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK_SCAN));
+ robottype_subscriber_sick551_scan_create(&orte,
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK551_SCAN));
robottype_subscriber_hokuyo_scan_create(&orte,
- generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
- robottype_subscriber_vidle_cmd_create(&orte,
- generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD));
+ generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN));
robottype_subscriber_fsm_main_create(&orte,
- rcv_fsm_main_cb, this);
+ rcv_fsm_main_cb, this);
robottype_subscriber_fsm_motion_create(&orte,
- rcv_fsm_motion_cb, this);
+ rcv_fsm_motion_cb, this);
robottype_subscriber_fsm_act_create(&orte,
- rcv_fsm_act_cb, this);
+ rcv_fsm_act_cb, this);
/* motors */
orte.motion_speed.left = 0;
orte.motion_speed.right = 0;
/* power management */
- orte.pwr_ctrl.voltage33 = true;
- orte.pwr_ctrl.voltage50 = true;
- orte.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage80 = true;
voltage33CheckBox->setChecked(true);
voltage50CheckBox->setChecked(true);
voltage80CheckBox->setChecked(true);
/* Get segment identificator in a read only mode */
segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
if(segmentId == -1) {
- QMessageBox::critical(this, "robomon",
- "Unable to open shared memory segment!");
- return;
+ statusBar->showMessage("No external map found - creating a new map.");
}
/* Init Shmap */
sharedMemoryOpened = true;
}
-double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
+double RobomonAtlantis::distanceToWallLidar(const struct lidar_params lidar, int beamnum)
{
- double distance=4.0, min_distance=4.0;
- int i,j;
+ double distance = 4.0, min_distance = 4.0;
+ int i, j;
Point wall;
struct map *map = ShmapIsMapInit();
- if (!map) return min_distance;
+ if (!map)
+ return min_distance;
// Simulate obstacles
- for(j=0;j<MAP_HEIGHT;j++) {
- for (i=0;i<MAP_WIDTH;i++) {
+ for(j = 0; j < MAP_HEIGHT; j++) {
+ for (i = 0; i < MAP_WIDTH; i++) {
struct map_cell *cell = &map->cells[j][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;
+ distance = distanceToObstacleLidar(lidar, 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)
+double RobomonAtlantis::distanceToCircularObstacleLidar(const struct lidar_params lidar, 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.x = lidar.center_offset_m;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.ang = index2rad(lidar, 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));
+ 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 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 = 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 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;
+ 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.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;
distance = (distance1 < distance2) ? distance1 : distance2;
} else if (D == 0) {
Point ob;
- ob.x = -B / (2*A);
+ 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)
}
/**
- * Calculation for Hokuyo simulation. Calculates distance that would
- * be returned by Hokuyo sensors, if there is only one obstacle (as
+ * Calculation for Lidar simulation. Calculates distance that would
+ * be returned by Lidar sensors, if there is only one obstacle (as
* specified by parameters).
*
- * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
+ * @param beamnum Lidar's bean number [0..LIDAR_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)
+double RobomonAtlantis::distanceToObstacleLidar(const struct lidar_params lidar, int beamnum, Point obstacle, double obstacleSize)
{
struct robot_pos_type e = orte.est_pos_best;
double sensor_a;
struct sharp_pos s;
- s.x = HOKUYO_CENTER_OFFSET_M;
+ s.x = lidar.center_offset_m;
s.y = 0.0;
- s.ang = HOKUYO_INDEX_TO_RAD(beamnum);
+ s.ang = index2rad(lidar, 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));
+ 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 = 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) - 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);
+ distance = distance / cos(angle);
}
if (distance > sensorRange)
distance = sensorRange;
void RobomonAtlantis::showShapeDetect(bool show)
{
- hokuyoScan->showShapeDetect = show;
+ hokuyoScan->showShapeDetect = show;
}
projects / eurobot / public.git / blobdiff