From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 2 May 2008 08:22:46 +0000 (+0200)
Subject: robomon: Obstacle simlation is working
X-Git-Tag: eb2008~54^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/9481b71d50dd05b01e17278569a771a6e36b3e9d

robomon: Obstacle simlation is working
---

diff --git a/src/robomon/src2/RobomonExplorer.cpp b/src/robomon/src2/RobomonExplorer.cpp
index c5fec59d..65a59f41 100644
--- 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 7541fed2..1d184477 100644
--- 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 6b0a021d..d411b76f 100644
--- 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/