From 0804a2fc37c8bbcb72a68e9cfd2353285333397e Mon Sep 17 00:00:00 2001
From: Miguel Angel Astor Romero <sonofgrendel@gmail.com>
Date: Sat, 18 Jun 2016 17:08:28 -0400
Subject: [PATCH] Added sensors to the robot. Correct exit on sigint.

---
 .gitignore  |  3 ++-
 Makefile    |  2 +-
 ant.cfg     |  8 ++++----
 ant.world   | 13 ++++++-------
 ant_bot.inc | 16 ++++++++++++++++
 ias_ss.cpp  | 16 +++++++++++++---
 robot.cpp   | 19 +++++++++++++++++--
 robot.hpp   |  1 +
 8 files changed, 60 insertions(+), 18 deletions(-)

diff --git a/.gitignore b/.gitignore
index 2305030..db766be 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
 *~
 *#
 *.o
-ias-ss
\ No newline at end of file
+*.d
+ias-ss
diff --git a/Makefile b/Makefile
index 66135be..e5813cc 100644
--- a/Makefile
+++ b/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
diff --git a/ant.cfg b/ant.cfg
index dfd7f11..661c482 100644
--- a/ant.cfg
+++ b/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"
 )
\ No newline at end of file
diff --git a/ant.world b/ant.world
index 7abfdd9..6609f42 100644
--- a/ant.world
+++ b/ant.world
@@ -3,18 +3,17 @@ include "ant_bot.inc"
 
 window
 (
-  size [ 635.000 666.000 ] # in pixels
-  scale 36.995   # pixels per meter
-  center [ -0.040  -0.274 ]
-  rotate [ 0  0 ]
-  			
-  show_data 1              # 1=on 0=off
+	size [ 635.000 666.000 ]
+	scale 36.995
+	center [ -0.040  -0.274 ]
+	rotate [ 0  0 ]
+	show_data 1
 )
 
 floorplan
 (
 	bitmap "maps/cave.png"
-	size [ 16.000 16.000 1.000 ]
+	size [ 16.000 16.000 1.500 ]
 )
 
 ant_bot
diff --git a/ant_bot.inc b/ant_bot.inc
index e9020c9..3741c6b 100644
--- a/ant_bot.inc
+++ b/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()
 )
diff --git a/ias_ss.cpp b/ias_ss.cpp
index ac8a7c1..5715ff3 100644
--- a/ias_ss.cpp
+++ b/ias_ss.cpp
@@ -34,20 +34,30 @@ 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;
-    robot->run();
+
+    while(!done)
+      robot->run();
 
     return NULL;
   }
 }
 
 int main(int argc, char **argv) {
-  pthread_t                robot_threads[NUM_ROBOTS];
-  std::vector<IASSS_Robot *>    robots;
+  pthread_t                  robot_threads[NUM_ROBOTS];
+  std::vector<IASSS_Robot *> robots;
+
+  signal(SIGINT, handler);
   
   try {
     // Initialize the robot objects and threads.
diff --git a/robot.cpp b/robot.cpp
index 82d785c..b06fa8e 100644
--- a/robot.cpp
+++ b/robot.cpp
@@ -2,11 +2,16 @@
 
 Robot::Robot(std::string hostname, uint32_t port) {
   _p_client = new PlayerCc::PlayerClient(hostname, port);
-  _p_proxy = new PlayerCc::Position2dProxy(_p_client, 0);
+  _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);
+  }
 }
diff --git a/robot.hpp b/robot.hpp
index ab4f3fd..64a9fb6 100644
--- a/robot.hpp
+++ b/robot.hpp
@@ -43,6 +43,7 @@ public:
 protected:
   PlayerCc::PlayerClient *    _p_client;
   PlayerCc::Position2dProxy * _p_proxy;
+  PlayerCc::RangerProxy *     _r_proxy;
 };
 
 /**