Better wall avoidance.

ant.world

@@ -35,8 +35,8 @@ window
 
 floorplan
 (
-	bitmap "maps/hospital_section.png"
-	size [ 64 26 1.500 ]
+	bitmap "maps/cave.png"
+	size [ 16 16 1.500 ]
 )
 
 ant_bot

ant_bot.inc

@@ -25,16 +25,14 @@ define ant_sensor sensor
 (
 	size [0.01 0.05 0.01 ] 
 	range [0 5.0]
-	fov 60
-	samples 1
+	fov 222
+	samples 222
 	color_rgba [ 0 1 0 0.2 ] 
 )
 
 define ant_sonars ranger
 (
-	ant_sensor( pose [ 0 0 0 -60 ] )
-	ant_sensor( pose [ 0 0 0   0 ] )
-	ant_sensor( pose [ 0 0 0  60 ] )
+	ant_sensor( pose [ 0 0 0 0 ] )
 )
 
 define ant_bot position

ias_robot.cpp

@@ -23,28 +23,63 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                      *
  *************************************************************************************/
 
+#include <limits>
+#include <sys/time.h>
+
 #include "ias_robot.hpp"
 
+static const long   HALF_SECOND_USEC = 500000;
+static const double MIN_DIST_M       = 1.5;
+
 IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port) : Robot(hostname, port) {
   std::cout << "Creating IAS-SS robot on host \"" << hostname << "\" and port " << port << "." << std::endl;
 }
 
 IASSS_Robot::~IASSS_Robot() {
-  std::cout << "Destroying IAS-SS robot." << std::endl;
+  std::cout << "Destroying IAS-SS robot on " << _host_name << ":" << _port << std::endl;
 }
 
 void IASSS_Robot::run() {
-  _p_client->Read();
+  int    rv;
+  long   then, now, delta, wait;
+  struct timeval tv;
+  double dist   = std::numeric_limits<double>::infinity();
+  double dist_l = 0.0;
+  double dist_r = 0.0;
 
-  if(_r_proxy->GetRange(1) < 1.0) {
-    if(_r_proxy->GetRange(0) > 1.0)
-      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-45));
-    else if(_r_proxy->GetRange(2) > 1.0)
-      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(45));
+  _p_client->Read();
+  rv = gettimeofday(&tv, NULL);
+  then = tv.tv_usec;
+  /******************************************************************************
+   * WALL AVOIDANCE START                                                       *
+   ******************************************************************************/
+  // Check if there is something in front of the robot.
+  for(int i = 96; i < 126; i++)
+    dist = _r_proxy->GetRange(i) < dist ? _r_proxy->GetRange(i) : dist;
+
+  if(dist < MIN_DIST_M) {
+    for(unsigned int i = 0; i < 96; i++)
+      dist_r += _r_proxy->GetRange(i);
+    dist_r /= 96;
+
+    for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++)
+      dist_l += _r_proxy->GetRange(i);
+    dist_l /= (_r_proxy->GetRangeCount() - 126);
+
+    if(dist_r >= dist_l)
+      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(-20));
     else
-      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(180));
-  } else {
-    _p_proxy->SetSpeed(0.3f, 0.0f);
-  }
-  sleep(1);
+      _p_proxy->SetSpeed(0.0f, PlayerCc::dtor(20));
+  } else
+    _p_proxy->SetSpeed(0.4f, 0.0f);
+  /******************************************************************************
+   * WALL AVOIDANCE END                                                         *
+   ******************************************************************************/
+  rv = gettimeofday(&tv, NULL);
+  now = tv.tv_usec;
+  delta = now - then;
+
+  // Sleep for a bit before finishing this control iteration.
+  wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC;
+  usleep(wait);
 }

ias_ss.cpp

@@ -35,21 +35,19 @@ const uint32_t    NUM_ROBOTS = 4;
 
 static bool done = false;
 
-extern "C" {
-  void handler(int signal) {
-    done = true;
-  }
+extern "C" void handler(int signal) {
+  done = true;
+}
   
-  void * robot_thread(void * arg) {
-    IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg);
+extern "C" void * robot_thread(void * arg) {
+  IASSS_Robot * robot = static_cast<IASSS_Robot *>(arg);
 
-    std::cout << "Running robot thread." << std::endl;
+  std::cout << "Running robot thread." << std::endl;
 
-    while(!done)
-      robot->run();
+  while(!done)
+    robot->run();
 
-    return NULL;
-  }
+  return NULL;
 }
 
 int main(int argc, char **argv) {

robot.cpp

@@ -26,9 +26,13 @@
 #include "robot.hpp"
 
 Robot::Robot(std::string hostname, uint32_t port) {
+  _host_name = hostname;
+  _port = 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();
   _r_proxy->RequestConfigure();

robot.hpp

@@ -41,6 +41,8 @@ public:
   virtual void run() = 0;
   
 protected:
+  int                         _port;
+  std::string                 _host_name;
   PlayerCc::PlayerClient *    _p_client;
   PlayerCc::Position2dProxy * _p_proxy;
   PlayerCc::RangerProxy *     _r_proxy;