Unify ORTE initialization

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

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index fdf734f4cb3d9e90f3a27e791868ef4f18a2f6c5..63270a711a1911992a5058d9cedd9d7e6ebe86ac 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);
 }
@@ -319,8 +322,10 @@ void RobomonAtlantis::createRobots()
 
 void RobomonAtlantis::createMap()
 {
-       mapImage = new Map(QPen(Qt::lightGray), QBrush(Qt::NoBrush));
-       mapImage->setZValue(3);
+       mapImage = new Map();
+       mapImage->setZValue(5);
+       mapImage->setTransform(QTransform().scale(MAP_CELL_SIZE_MM, MAP_CELL_SIZE_MM), true);
+
 
        playgroundScene->addItem(mapImage);
 }
@@ -348,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;
@@ -361,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)
@@ -455,8 +474,8 @@ void RobomonAtlantis::paintMap()
 
         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];
@@ -490,7 +509,8 @@ void RobomonAtlantis::paintMap()
                         if (cell->flags & MAP_FLAG_DET_OBST)
                                 color = cyan;
 
-                        mapImage->setPixelColor(i, j, color);
+                       color.setAlpha(200);
+                        mapImage->setPixelColor(i, MAP_HEIGHT - j - 1, color);
                 }
        }
 }
@@ -536,7 +556,8 @@ void RobomonAtlantis::simulateObstaclesHokuyo()
 
        for (i=0; i<HOKUYO_ARRAY_SIZE; i++) {
                wall_distance = distanceToWallHokuyo(i);
-               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;
@@ -569,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();
@@ -750,8 +774,6 @@ void RobomonAtlantis::createOrte()
 {
        int rv;
 
-       orte.strength = 11;
-
        memset(&orte, 0, sizeof(orte));
        rv = robottype_roboorte_init(&orte);
        if (rv) {
@@ -780,8 +802,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,
@@ -898,6 +920,69 @@ double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
        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