robomon: Add button for strategy switching.

[eurobot/public.git] / src / robomon / RobomonAtlantis.cpp

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index d48c466b4ba23cd602897efef0cecd3eafbd726e..7995af1d8108856a40a11fb9ca87b99b71dac23b 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -182,6 +182,9 @@ void RobomonAtlantis::createMiscGroupBox()
 
        colorChoser = new QCheckBox("&Team color");
        layout->addWidget(colorChoser);
+        
+        strategyButton= new QPushButton(tr("Strategy"));
+        layout->addWidget(strategyButton);
 
        miscGroupBox->setLayout(layout);
 }
@@ -350,6 +353,8 @@ void RobomonAtlantis::createActions()
 
        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;
@@ -363,6 +368,18 @@ void RobomonAtlantis::createActions()
                        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)
@@ -540,9 +557,7 @@ void RobomonAtlantis::simulateObstaclesHokuyo()
        for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
                wall_distance = distanceToWallHokuyo(i);
 
-               // TODO: Replace with function to calculate the real distance to the simulated cylinder
-               distance = realDistanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
-               //distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
+               distance = distanceToCircularObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
                if (wall_distance < distance)
                        distance = wall_distance;
                hokuyo[i] = distance*1000;
@@ -575,8 +590,11 @@ bool RobomonAtlantis::event(QEvent *event)
                case QEVENT(QEV_HOKUYO_SCAN):
                        hokuyoScan->newScan(&orte.hokuyo_scan);
                        break;
-               case QEVENT(QEV_VIDLE_CMD):
-                       robotEstPosBest->setVidle(orte.vidle_cmd.req_pos);
+               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();
@@ -786,8 +804,8 @@ void RobomonAtlantis::createOrte()
                        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_vidle_cmd_create(&orte,
-                       generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD));
+       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,
@@ -904,7 +922,7 @@ double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
        return min_distance;
 }
 
-double RobomonAtlantis::realDistanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
+double RobomonAtlantis::distanceToCircularObstacleHokuyo(int beamnum, Point center, double diameter)
 {
        struct robot_pos_type e = orte.est_pos_best;
        double sensor_a;
@@ -923,18 +941,21 @@ double RobomonAtlantis::realDistanceToObstacleHokuyo(int beamnum, Point obstacle
        double distance = sensorRange;
        double angle;
 
-       angle = sensor.angleTo(obstacle) - sensor_a;
+       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 = obstacleSize / 2.0;
+       double r = diameter / 2.0;
 
        double A = 1 + k*k;
-       double B = 2 * (sensor.y*k - obstacle.x - k*k*sensor.x - obstacle.y*k);
-       double C = obstacle.x*obstacle.x + obstacle.y*obstacle.y + k*k*sensor.x*sensor.x - 2*sensor.y*k*sensor.x + sensor.y*sensor.y + 2*k*sensor.x*obstacle.y - 2*sensor.y*obstacle.y - r*r;
+       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;
        
@@ -949,26 +970,17 @@ double RobomonAtlantis::realDistanceToObstacleHokuyo(int beamnum, Point obstacle
                double distance1 = sensor.distanceTo(ob1);
                double distance2 = sensor.distanceTo(ob2);
                distance = (distance1 < distance2) ? distance1 : distance2;
-       }
-
-       if (D == 0) {
+       } else if (D == 0) {
                Point ob;
                ob.x = -B / (2*A);
                ob.y = k * (ob.x - sensor.x) + sensor.y;
                distance = sensor.distanceTo(ob);
        }
-
-       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) {
-                       distance = sensorRange;
-               }
-       } else {
+       distance = distance + (drand48()-0.5)*3.0e-2;
+       if (D < 0 || angle > atan(r / distance))
+               distance = sensorRange;
+       if (distance > sensorRange)
                distance = sensorRange;
-       }
 
        return distance;
 }