robomon: add function to calculate the real distance to the simulated cylinder

diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp
index 52c787f3aecfc388f7b61f9c83cf8f68edd38525..d48c466b4ba23cd602897efef0cecd3eafbd726e 100644 (file)
--- a/src/robomon/RobomonAtlantis.cpp
+++ b/src/robomon/RobomonAtlantis.cpp
@@ -541,7 +541,8 @@ void RobomonAtlantis::simulateObstaclesHokuyo()
                wall_distance = distanceToWallHokuyo(i);
 
                // TODO: Replace with function to calculate the real distance to the simulated cylinder
-               distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
+               distance = realDistanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M);
+               //distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
                if (wall_distance < distance)
                        distance = wall_distance;
                hokuyo[i] = distance*1000;
@@ -903,6 +904,75 @@ double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
        return min_distance;
 }
 
+double RobomonAtlantis::realDistanceToObstacleHokuyo(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.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(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);
+
+       double k = tan(sensor_a);
+       double r = obstacleSize / 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 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;
+       }
+
+       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 = sensorRange;
+       }
+
+       return distance;
+}
+
 /**
  * Calculation for Hokuyo simulation. Calculates distance that would
  * be returned by Hokuyo sensors, if there is only one obstacle (as

diff --git a/src/robomon/RobomonAtlantis.h b/src/robomon/RobomonAtlantis.h
index f8756f46612e63d036ae3b4d03d68a774c817cdc..e42ff677fbfa80c7dbd3571b8bf90df6e2439cd4 100644 (file)
--- a/src/robomon/RobomonAtlantis.h
+++ b/src/robomon/RobomonAtlantis.h
@@ -192,6 +192,7 @@ private:
        /* obstacle simulation */
        double distanceToWallHokuyo(int beamnum);
        double distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize);
+       double realDistanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize);
        int simulationEnabled;
 
        QTimer *obstacleSimulationTimer;