Added sensors to the robot. Correct exit on sigint.

.gitignore

@@ -1,4 +1,5 @@
 *~
 *#
 *.o
+*.d
 ias-ss

Makefile

@@ -1,5 +1,5 @@
 CXX = g++
-TARGET = ias_ss
+TARGET = ias-ss
 OBJECTS = ias_ss.o robot.o
 CFLAGS = -ansi -pedantic -Wall -g `pkg-config --cflags playerc++` -g
 LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread

ant.cfg

@@ -9,27 +9,27 @@ driver
 driver
 (
 	name "stage"
-	provides [ "6666:position2d:0" ]
+	provides [ "6666:position2d:0" "6666:ranger:0" ]
 	model "ant_robot_1"
 )
 
 driver
 (
 	name "stage"
-	provides [ "6667:position2d:0" ]
+	provides [ "6667:position2d:0" "6667:ranger:0" ]
 	model "ant_robot_2"
 )
 
 driver
 (
 	name "stage"
-	provides [ "6668:position2d:0" ]
+	provides [ "6668:position2d:0" "6668:ranger:0" ]
 	model "ant_robot_3"
 )
 
 driver
 (
 	name "stage"
-	provides [ "6669:position2d:0" ]
+	provides [ "6669:position2d:0" "6669:ranger:0"]
 	model "ant_robot_4"
 )

ant.world

@@ -3,18 +3,17 @@ include "ant_bot.inc"
 
 window
 (
-  size [ 635.000 666.000 ] # in pixels
+	size [ 635.000 666.000 ]
-  scale 36.995   # pixels per meter
+	scale 36.995
 	center [ -0.040  -0.274 ]
 	rotate [ 0  0 ]
-  			
+	show_data 1
-  show_data 1              # 1=on 0=off
 )
 
 floorplan
 (
 	bitmap "maps/cave.png"
-	size [ 16.000 16.000 1.000 ]
+	size [ 16.000 16.000 1.500 ]
 )
 
 ant_bot

ant_bot.inc

@@ -1,3 +1,17 @@
+define ant_sensor sensor
+(
+	size [0.01 0.05 0.01 ] 
+	range [0 5.0]
+	fov 180
+	samples 180
+	color_rgba [ 0 1 0 0.2 ] 
+)
+
+define ant_sonars ranger
+(
+	ant_sensor( pose [ 0 0 0 0 ] )
+)
+
 define ant_bot position
 (
 	block
@@ -14,4 +28,6 @@ define ant_bot position
 
 	drive "diff"
 	localization "gps"
+
+	ant_sonars()
 )

ias_ss.cpp

@@ -34,11 +34,19 @@ const std::string HOST_NAME = "localhost";
 const uint32_t    PORT = 6666;
 const uint32_t    NUM_ROBOTS = 4;
 
+static bool done = false;
+
 extern "C" {
+  void handler(int signal) {
+    done = true;
+  }
+  
   void * robot_thread(void * arg) {
     IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg);
 
     std::cout << "Running robot thread." << std::endl;
+
+    while(!done)
       robot->run();
 
     return NULL;
@@ -49,6 +57,8 @@ int main(int argc, char **argv) {
   pthread_t                  robot_threads[NUM_ROBOTS];
   std::vector<IASSS_Robot *> robots;
 
+  signal(SIGINT, handler);
+  
   try {
     // Initialize the robot objects and threads.
     for(uint32_t i = 0; i < NUM_ROBOTS; ++i) {

robot.cpp

@@ -3,10 +3,15 @@
 Robot::Robot(std::string hostname, uint32_t port) {
   _p_client = new PlayerCc::PlayerClient(hostname, port);
   _p_proxy  = new PlayerCc::Position2dProxy(_p_client, 0);
+  _r_proxy  = new PlayerCc::RangerProxy(_p_client, 0);
+  _p_proxy->RequestGeom();
+  _r_proxy->RequestGeom();
 }
 
 Robot::~Robot() {
+  std::cout << "Destroying robot" << std::endl;
   delete _p_proxy;
+  delete _r_proxy;
   delete _p_client;
 }
 
@@ -19,5 +24,15 @@ IASSS_Robot::~IASSS_Robot() {
 }
 
 void IASSS_Robot::run() {
-  sleep(10);
+  _p_client->Read();
+
+  if(_r_proxy->GetRange(0) < 0.2) {
+    _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-45));
+    sleep(1);
+  } else if(_r_proxy->GetRange(_r_proxy->GetRangeCount()) < 0.2) {
+    _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(45));
+    sleep(1);
+  } else {
+    _p_proxy->SetSpeed(0.3f, 0.0f);
+  }
 }

robot.hpp

@@ -43,6 +43,7 @@ public:
 protected:
   PlayerCc::PlayerClient *    _p_client;
   PlayerCc::Position2dProxy * _p_proxy;
+  PlayerCc::RangerProxy *     _r_proxy;
 };
 
 /**