X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/4d8a5381299a7fa399dc242bdf87bebeaee70747..d3f518a5fcbbd8c4fe0a19c20d6a4162403b8ae0:/src/robomon/RobomonAtlantis.cpp diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp index a8560fe9..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,7 +81,7 @@ RobomonAtlantis::RobomonAtlantis(QWidget *parent) void RobomonAtlantis::createLeftLayout() { leftLayout = new QVBoxLayout(); - + createDebugGroupBox(); debugWindowEnabled = true; createPlaygroundGroupBox(); @@ -88,7 +92,7 @@ void RobomonAtlantis::createLeftLayout() void RobomonAtlantis::createRightLayout() { rightLayout = new QVBoxLayout(); - + createPositionGroupBox(); createMiscGroupBox(); createFSMGroupBox(); @@ -125,7 +129,7 @@ void RobomonAtlantis::createPositionGroupBox() positionGroupBox = new QGroupBox(tr("Position state")); positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); QGridLayout *layout = new QGridLayout(); - + actPosX = new QLineEdit(); actPosY = new QLineEdit(); actPosPhi = new QLineEdit(); @@ -133,7 +137,7 @@ void RobomonAtlantis::createPositionGroupBox() estPosX = new QLineEdit(); estPosY = new QLineEdit(); estPosPhi = new QLineEdit(); - + actPosX->setReadOnly(true); actPosY->setReadOnly(true); actPosPhi->setReadOnly(true); @@ -175,10 +179,13 @@ void RobomonAtlantis::createMiscGroupBox() 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); } @@ -223,16 +230,16 @@ void RobomonAtlantis::createActuatorsGroupBox() actuatorsGroupBox = new QGroupBox(tr("Actuators")); actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred); QHBoxLayout *layout = new QHBoxLayout(); - vidle = new QDial(); +// vidle = new QDial(); - vidle->setMinimum(VIDLE_VYSIP); - vidle->setMaximum(2*(VIDLE_UP-VIDLE_VYSIP)+VIDLE_VYSIP); - vidle->setEnabled(false); +// 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(vidle); //layout->addWidget(enginesGroupBox); actuatorsGroupBox->setLayout(layout); } @@ -274,53 +281,13 @@ void RobomonAtlantis::createPowerGroupBox() powerGroupBox->setLayout(layout); } -#if 0 -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); -} -#endif - void RobomonAtlantis::createRobots() { robotRefPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush)); robotRefPos->setZValue(11); trailRefPos = new Trail(QPen(Qt::darkBlue)); trailRefPos->setZValue(11); - + robotEstPosBest = new Robot("Est", QPen(), QBrush(Qt::darkGray)); robotEstPosBest->setZValue(10); trailEstPosBest = new Trail(QPen()); @@ -340,9 +307,9 @@ void RobomonAtlantis::createRobots() playgroundScene->addItem(robotEstPosBest); playgroundScene->addItem(robotEstPosIndepOdo); playgroundScene->addItem(robotEstPosOdo); - + showTrails(false); - + playgroundScene->addItem(trailRefPos); playgroundScene->addItem(trailPosIndepOdo); playgroundScene->addItem(trailOdoPos); @@ -350,7 +317,17 @@ void RobomonAtlantis::createRobots() hokuyoScan = new HokuyoScan(); hokuyoScan->setZValue(10); playgroundScene->addItem(hokuyoScan); - + +} + +void RobomonAtlantis::createMap() +{ + mapImage = new Map(); + mapImage->setZValue(5); + mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true); + + + playgroundScene->addItem(mapImage); } /********************************************************************** @@ -359,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)), +// connect(leftMotorSlider, SIGNAL(valueChanged(int)), // this, SLOT(setLeftMotor(int))); -// connect(rightMotorSlider, SIGNAL(valueChanged(int)), +// connect(rightMotorSlider, SIGNAL(valueChanged(int)), // this, SLOT(setRightMotor(int))); -// connect(stopMotorsPushButton, SIGNAL(clicked()), +// 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)), @@ -385,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) @@ -417,32 +408,32 @@ void RobomonAtlantis::setVoltage80(int state) // { // 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() @@ -462,7 +453,7 @@ void RobomonAtlantis::showMap(bool show) if (sharedMemoryOpened == false) return; - + if (show) { mapTimer = new QTimer(this); connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap())); @@ -473,7 +464,7 @@ void RobomonAtlantis::showMap(bool show) disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap())); } } - playgroundScene->showMap(show); + mapImage->setVisible(show); } void RobomonAtlantis::paintMap() @@ -482,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; @@ -517,15 +508,16 @@ 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) { + if(state) { robottype_publisher_hokuyo_scan_create(&orte, NULL, this); } else { if (!simulationEnabled) @@ -535,7 +527,7 @@ void RobomonAtlantis::setSimulation(int state) simulationEnabled = state; } -/*! +/*! \fn RobomonAtlantis::setObstacleSimulation(int state) */ void RobomonAtlantis::setObstacleSimulation(int state) @@ -544,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; } @@ -561,16 +553,17 @@ void RobomonAtlantis::simulateObstaclesHokuyo() double distance, wall_distance; 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; @@ -606,25 +605,25 @@ bool RobomonAtlantis::event(QEvent *event) 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, + 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, + 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, + 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, + trailEstPosBest->addPoint(QPointF(orte.est_pos_best.x, orte.est_pos_best.y)); - hokuyoScan->setPosition(orte.est_pos_best.x, + hokuyoScan->setPosition(orte.est_pos_best.x, orte.est_pos_best.y, - orte.est_pos_best.phi); + orte.est_pos_best.phi); break; case QEVENT(QEV_POWER_VOLTAGE): emit powerVoltageReceivedSignal(); @@ -776,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"); @@ -790,25 +790,27 @@ void RobomonAtlantis::createOrte() 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_odo_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, + 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, + robottype_subscriber_est_pos_best_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST)); - robottype_subscriber_hokuyo_scan_create(&orte, + robottype_subscriber_hokuyo_scan_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN)); - robottype_subscriber_fsm_main_create(&orte, + 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 */ @@ -826,11 +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(powerVoltageReceivedSignal()), + connect(this, SIGNAL(powerVoltageReceivedSignal()), this, SLOT(powerVoltageReceived())); } @@ -846,7 +848,7 @@ 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)); - robotRefPos->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)); } @@ -876,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) { @@ -884,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); @@ -902,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). @@ -928,11 +993,11 @@ 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_best; double sensor_a; struct sharp_pos s; @@ -944,11 +1009,11 @@ double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, do 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; @@ -960,7 +1025,7 @@ double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, do if (angle < M_PI/2.0) { distance = distance/cos(angle); } - if (distance > sensorRange) + if (distance > sensorRange) distance = sensorRange; } else { distance = sensorRange; @@ -996,3 +1061,8 @@ void RobomonAtlantis::showTrails(bool show) trailPosIndepOdo->setVisible(show && robotEstPosIndepOdo->isVisible()); trailOdoPos->setVisible(show && robotEstPosOdo->isVisible()); } + +void RobomonAtlantis::showShapeDetect(bool show) +{ + hokuyoScan->showShapeDetect = show; +}