robomon: Obstacle simlation is working

diff --git a/src/robomon/src2/RobomonExplorer.cpp b/src/robomon/src2/RobomonExplorer.cpp
index c5fec59d3b4e1e161a8516387f2ca70d0847cdaa..65a59f410f51288585153540f7e7cdfd327a8e63 100644 (file)
--- a/src/robomon/src2/RobomonExplorer.cpp
+++ b/src/robomon/src2/RobomonExplorer.cpp
@@ -24,7 +24,7 @@
 
 #include <orte.h>
 #include <path_planner.h>
-#include <robodim_eb2007.h>
+#include <robodim_eb2008.h>
 #include <sharp.h>
 #include <trgen.h>
 #include <map.h>
@@ -786,15 +786,13 @@ void RobomonExplorer::setSharpValues(int value)
 
 void RobomonExplorer::setSimulation(int state)
 {
-       // FIXME
        if(state) { 
-//             createSharpLongsPublisher(this, &orteData);
-//             createSharpShortsPublisher(this, &orteData);
+               eb2008_publisher_sharps_create(&orte_eb2008, 
+                                              dummy_publisher_callback, this);
        } else {
                if (!simulationEnabled)
                        return;
-//             ORTEPublicationDestroy(orteData.publisherSharpShort);
-//             ORTEPublicationDestroy(orteData.publisherSharpLong);
+               eb2008_publisher_sharps_destroy(&orte_eb2008);
        }
        simulationEnabled = state;
 }
@@ -816,11 +814,11 @@ void RobomonExplorer::setObstacleSimulation(int state)
                if (obstacleSimulationTimer) 
                        delete obstacleSimulationTimer;
                double distance = 0.8;
-               // FIXME
-/*             orteData.orteSharpOpponent.longSharpDist1 = distance;
-               orteData.orteSharpOpponent.longSharpDist2 = distance;
-               orteData.orteSharpOpponent.longSharpDist3 = distance;
-               ORTEPublicationSend(orteData.publisherSharpLong);*/
+               orte_eb2008.sharps.left = distance;
+               orte_eb2008.sharps.front_left = distance;
+               orte_eb2008.sharps.front_right = distance;
+               orte_eb2008.sharps.right = distance;
+               ORTEPublicationSend(orte_eb2008.publication_sharps);
        }
 }
 
@@ -830,11 +828,24 @@ void RobomonExplorer::setObstacleSimulation(int state)
 void RobomonExplorer::simulateObstacles()
 {
        double distance, wall_distance;
+       int i;
+       double *sharp[4] = {
+               &orte_eb2008.sharps.left,
+               &orte_eb2008.sharps.right,
+               &orte_eb2008.sharps.front_left,
+               &orte_eb2008.sharps.front_right
+       };
 
-       wall_distance = distanceToWall();
-       distance = distanceToObstacle(simulatedObstacle, 0.5/*meters*/);
-       if (wall_distance < distance) 
-               distance = wall_distance;
+       
+       for (i=0; i<4; i++) {
+               wall_distance = distanceToWall(i);
+               distance = distanceToObstacle(i, simulatedObstacle, 0.5/*meters*/);
+               if (wall_distance < distance) 
+                       distance = wall_distance;
+               *sharp[i] = distance;
+       }
+       ORTEPublicationSend(orte_eb2008.publication_sharps);
+       
        // FIXME
        /*
        orteData.orteSharpOpponent.longSharpDist1 = distance;
@@ -1222,9 +1233,9 @@ void RobomonExplorer::openSharedMemory()
        sharedMemoryOpened = true;
 }
 
-double RobomonExplorer::distanceToWall()
+double RobomonExplorer::distanceToWall(int sharpnum)
 {
-       double distance=1.0, min_distance=1.0;
+       double distance=0.8, min_distance=0.8;
        int i,j;
        Point wall;
        struct map *map = ShmapIsMapInit();
@@ -1239,7 +1250,7 @@ double RobomonExplorer::distanceToWall()
                                // WALL
                                ShmapCell2Point(i, j, &wall.x, &wall.y);
                                
-                               distance = distanceToObstacle(wall, MAP_CELL_SIZE_M);
+                               distance = distanceToObstacle(sharpnum, wall, MAP_CELL_SIZE_M);
                                if (distance<min_distance) min_distance = distance;
                        }
                }
@@ -1258,25 +1269,30 @@ double RobomonExplorer::distanceToWall()
  * 
  * @return Distance measured by sensors in meters.
  */    
-double RobomonExplorer::distanceToObstacle(Point obstacle, double obstacleSize)
+double RobomonExplorer::distanceToObstacle(int sharpnum, Point obstacle, double obstacleSize)
 {  
-       Point robot(orte_generic.ref_pos.x, orte_generic.ref_pos.y);
-       const double sensorRange = 1.0; /*[meters]*/
-                           
+       struct est_pos_type e = orte_generic.est_pos;
+       double sensor_a;
+       struct sharp_pos s = sharp[sharpnum];
+       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 = 0.8; /*[meters]*/
+       
        double distance, angle;
            
-       angle = robot.angleTo(obstacle) - orte_generic.ref_pos.phi;
+       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);
-       distance = robot.distanceTo(obstacle)-0.11;
+       distance = sensor.distanceTo(obstacle)-0.11;
        if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
                // We can see the obstackle from here.
                if (angle < M_PI/2.0) {
                    distance = distance/cos(angle);
                }
-               distance -= ROBOT_AXIS_TO_BELT_M;
                if (distance > sensorRange) 
                        distance = sensorRange;
        } else {

diff --git a/src/robomon/src2/RobomonExplorer.h b/src/robomon/src2/RobomonExplorer.h
index 7541fed2e479aa58959b902c3105cd847f48961a..1d1844777f80b2a45cc6fb1fd57c0467274a5f54 100644 (file)
--- a/src/robomon/src2/RobomonExplorer.h
+++ b/src/robomon/src2/RobomonExplorer.h
@@ -234,8 +234,8 @@ private:
        QTimer *mapTimer;
 
        /* obstacle simulation */
-       double distanceToWall();
-       double distanceToObstacle(Point obstacle, double obstacleSize);
+       double distanceToWall(int sharpnum);
+       double distanceToObstacle(int sharpnum, Point obstacle, double obstacleSize);
        int simulationEnabled;
 
        QTimer *obstacleSimulationTimer;

diff --git a/src/robomon/src2/src2.pro b/src/robomon/src2/src2.pro
index 6b0a021dde416e96f8c2bf1170fab5cd4fb26ce6..d411b76f84443c06c917f02aa265e54fb7dfb5d5 100644 (file)
--- a/src/robomon/src2/src2.pro
+++ b/src/robomon/src2/src2.pro
@@ -45,7 +45,7 @@ HEADERS += MainWindow.h \
            MiscGui.h \
            robomon_orte.h
 
-LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim_eb2008 -lrobomath -lroboorte_generic -lroboorte_eb2008 -lroboorte_eb2007 -lrobottype -lrobottype_eb2007 -lrobottype_eb2008 -lorte  -lsharp -lmap
+LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim_eb2008 -lrobomath -lroboorte_generic -lroboorte_eb2008 -lroboorte_eb2007 -lrobottype -lrobottype_eb2007 -lrobottype_eb2008 -lorte  -lsharp -lmap -lrobodim_eb2008
 
 OBJECTS_DIR = ../../../build/linux/_build/user/robomon2/
 MOC_DIR = ../../../build/linux/_build/user/robomon2/