Makers

make.sh

@@ -0,0 +1 @@
+./workspace build:debug && ./workspace tests:debug

src/ai/CMakeLists.txt

@@ -43,6 +43,10 @@ set (SOURCES
     core/export_to_plot.cpp
     core/logger.cpp
     referee/Referee.cpp
+    makers/avoid_obstacle.cpp
+    makers/algo1.cpp
+    makers/space_discretization.cpp
+    makers/Astar.cpp
 )
 
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/../../bin/)
@@ -73,6 +77,10 @@ if(CATKIN_ENABLE_TESTING)
     control/test_pid.cpp
     control/test_robot_control.cpp
     ai/robot_behavior/test_robot_behavior.cpp
+    makers/test_avoid_obstacle.cpp
+    makers/test_algo1.cpp
+    makers/test_space_discretization.cpp
+    makers/test_Astar.cpp
   )
 
   foreach(test_source ${TEST_SOURCES})

src/ai/makers/Astar.cpp

@@ -0,0 +1,126 @@
+#include "Astar.h"
+
+Grid::Grid(int X, int Y, double eX, double eY, pair<int,int> s, pair<int,int> g, vector< pair<int,int> > o):X(X),Y(Y),edgeX(eX),edgeY(eY),start(s),goal(g){
+  vector< pair<int,int> >::const_iterator it;
+  for(it = o.begin(); it != o.end(); ++it){
+    obstacles[(*it)] = 1;
+  }
+}
+
+int Grid::heuristic_cost_estimate(pair<int,int> p1, pair<int,int> p2){
+  return abs(p1.first-p2.first)+abs(p1.second-p2.second);
+}
+
+vector< pair<int,int> > Grid::findN(pair<int,int> p){
+  vector< pair<int,int> > v;
+  if(p.first+1 <= X){
+    v.push_back(make_pair(p.first+1,p.second));
+  }
+  if(p.second+1 <= Y){
+    v.push_back(make_pair(p.first,p.second+1));
+  }
+  if(p.first-1 >= 1){
+    v.push_back(make_pair(p.first-1,p.second));
+  }
+  if(p.second-1 >= 1){
+    v.push_back(make_pair(p.first,p.second-1));
+  }
+  if(p.first+1 <= X && p.second+1 <= Y){
+    v.push_back(make_pair(p.first+1,p.second+1));
+  }
+  if(p.first+1 <= X && p.second-1 >= 1){
+    v.push_back(make_pair(p.first+1,p.second-1));
+  }
+  if(p.first-1 >= 1 && p.second+1 <= Y){
+    v.push_back(make_pair(p.first-1,p.second+1));
+  }
+  if(p.first-1 >= 1 && p.second-1 >= 1){
+    v.push_back(make_pair(p.first-1,p.second-1));
+  }
+  return v;
+}
+
+int Grid::dist_between(pair<int,int> p1, pair<int,int> p2){
+  if(abs(p1.first-p2.first)+abs(p1.second-p2.second)==1){
+    return 10;
+  }
+  else{
+    return 14;
+  }
+}
+
+vector< pair<int,int> > Grid::reconstruct_path(map< pair<int,int>, pair<int,int> > m, pair<int,int> p){
+  vector< pair<int,int> > vp;
+  vp.push_back(p);
+  while(m.find(p) != m.end()){
+    p = m[p];
+    vp.push_back(p);
+  }
+  return vp;
+}
+
+vector< pair<int,int> > Grid::A(){
+  set< pair<int,int> > closedSet;
+  set< pair<int,int> > openSet;
+  openSet.insert(start);
+  map< pair<int,int>, pair<int,int> > cameFrom;
+  map< pair<int,int>, int> gScore;
+  for(int i = 1; i <= X; i++){
+    for(int j = 1; j <= Y; j++){
+      gScore[make_pair(i,j)] = INFINITY;
+    }
+  }
+  gScore[start] = 0;
+  map<pair<int,int>,int> fScore;
+  for(int i = 1; i <= X; i++){
+    for(int j = 1; j <= Y; j++){
+      fScore[make_pair(i,j)] = INFINITY;
+    }
+  }
+  fScore[start] = heuristic_cost_estimate(start,goal);
+  pair<int,int> current;
+  int min;
+  vector< pair<int,int> > neighbors;
+  int tentative_gScore;
+  while(!openSet.empty()){
+    min = INFINITY;
+    for(set< pair<int,int> >::const_iterator is = openSet.begin(); is != openSet.end(); ++is){
+      if(fScore[(*is)] < min){
+	min = fScore[(*is)];
+	current = (*is);
+      }
+    }
+    if(current == goal){
+      return reconstruct_path(cameFrom,current);
+    }
+    openSet.erase(current);
+    closedSet.insert(current);
+    neighbors = findN(current);
+    for(vector< pair<int,int> >::const_iterator in = neighbors.begin(); in != neighbors.end(); ++in){
+      if(closedSet.find(*in)!=closedSet.end() || obstacles.find(*in)!=obstacles.end()){
+	continue;
+      }
+      if(openSet.find(*in)==openSet.end()){
+	openSet.insert(*in);
+      }
+      tentative_gScore = gScore[current] + dist_between(current,*in);
+      if(tentative_gScore < gScore[*in]){
+	cameFrom[*in] = current;
+	gScore[*in] = tentative_gScore;
+	fScore[*in] = gScore[*in] + heuristic_cost_estimate(*in,goal);
+      }
+    }
+  }
+  vector< pair<int,int> > b;
+  return b;
+}
+
+vector< pair<double,double> > Grid::Astar(){
+  vector< pair<int,int> > p = A();
+  vector< pair<double,double> > d;
+  for(vector< pair<int,int> >::iterator it = p.begin(); it!=p.end(); ++it){
+    d.push_back(make_pair(((it->first)-1)*edgeX+edgeX/2,((it->second)-1)*edgeY+edgeY/2));
+  }
+  return d;
+}
+

src/ai/makers/Astar.h

@@ -0,0 +1,42 @@
+#ifndef __ASTAR_H__
+#define __ASTAR_H__
+
+#include <iostream>
+#include <cstring>
+#include <stdlib.h>
+#include <utility>
+#include <map>
+#include <vector>
+#include <string>
+#include <set>
+#include <cmath>
+#include <algorithm>
+
+using namespace std;
+
+class Grid{
+public:
+  int X;
+  int Y;
+  double edgeX;
+  double edgeY;
+  pair<int,int> start;
+  pair<int,int> goal;
+  map<pair<int,int>,int> obstacles;
+
+  Grid(int X, int Y, double eX, double eY, pair<int,int> s, pair<int,int> g, vector< pair<int,int> > o);
+
+  int heuristic_cost_estimate(pair<int,int> p1, pair<int,int> p2);
+
+  vector< pair<int,int> > findN(pair<int,int> p);
+
+  int dist_between(pair<int,int> p1, pair<int,int> p2);
+
+  vector< pair<int,int> > reconstruct_path(map< pair<int,int>, pair<int,int> > m, pair<int,int> p);
+
+  vector< pair<int,int> > A();
+
+  vector< pair<double,double> > Astar();
+};
+
+#endif /* __ASTAR_H__ */

src/ai/makers/Obstacle.h

@@ -0,0 +1,13 @@
+#ifndef __OBSTACLE_H__
+#define __OBSTACLE_H__
+
+typedef struct Obstacle Obstacle;
+
+struct Obstacle {
+    double radius;
+    rhoban_geometry::Point linear_position;
+
+    Obstacle( double radius, const rhoban_geometry::Point & linear_position );
+};
+
+#endif /* __OBSTACLE_H__ */

src/ai/makers/algo1.cpp

@@ -0,0 +1,27 @@
+#include "algo1.h"
+
+Algo1::Algo1( double discretisation_size ):
+   discretisation_size( discretisation_size )
+{
+}
+
+void Algo1::set_constraint(
+    double width_field, double height_field,
+    const rhoban_geometry::Point & starting_linear_position,
+    const Vector2d & starting_linear_velocity,
+    const rhoban_geometry::Point & ending_linear_position,
+    const Vector2d & ending_linear_velocity,
+    double robot_radius,
+    const std::list< Obstacle > & obstacles, 
+    double minimal_radius_curve
+){
+}
+
+/*
+  This function take a parameter u in [0, 1.0] and return the postion 
+  of the robot.
+*/
+rhoban_geometry::Point Algo1::linear_position( double u ) const {
+	return rhoban_geometry::Point( 0.0, 0.0 );
+}
+

src/ai/makers/algo1.h

@@ -0,0 +1,31 @@
+#ifndef __ALGO1__H_
+#define __ALGO1__H_
+
+#include "avoid_obstacle.h"
+
+class Algo1 : Avoid_obstacle {
+      public:
+      double discretisation_size;
+
+
+      Algo1( double discretisation_size);
+
+      virtual void set_constraint(
+            double width_field, double height_field,
+            const rhoban_geometry::Point & starting_linear_position,
+            const Vector2d & starting_linear_velocity,
+            const rhoban_geometry::Point & ending_linear_position,
+            const Vector2d & ending_linear_velocity,
+            double robot_radius,
+            const std::list< Obstacle > & obstacles, 
+            double minimal_radius_curve
+      );
+
+      /*
+          This function take a parameter u in [0, 1.0] and return the postion 
+          of the robot.
+      */
+      virtual rhoban_geometry::Point linear_position( double u ) const;
+};
+
+#endif

src/ai/makers/avoid_obstacle.cpp

@@ -0,0 +1,10 @@
+#include "avoid_obstacle.h"
+
+Obstacle::Obstacle( double radius, const rhoban_geometry::Point & linear_position ): 
+    radius(radius),
+    linear_position(linear_position)
+{ }
+
+Avoid_obstacle::~Avoid_obstacle(){
+}
+

src/ai/makers/avoid_obstacle.h

@@ -0,0 +1,36 @@
+#ifndef __AVOID_OBSTACLE__H__
+#define __AVOID_OBSTACLE__H__
+
+#include <rhoban_geometry/point.h>
+#include <math/vector.h>
+#include <list>
+#include "Obstacle.h"
+
+class Avoid_obstacle {
+   public:
+
+      /* 
+         This function is used to configurate the datas of the obstacle and of some constraint
+         on the robot in order to compute a move for the robot.
+       */
+      virtual void set_constraint(
+            double width_field, double height_field,
+            const rhoban_geometry::Point & starting_linear_position,
+            const Vector2d & starting_linear_velocity,
+            const rhoban_geometry::Point & ending_linear_position,
+            const Vector2d & ending_linear_velocity,
+            double robot_radius,
+            const std::list< Obstacle > & obstacles, 
+            double minimal_radius_curve
+      ) = 0;
+
+      /*
+          This function take a parameter u in [0, 1.0] and return the postion 
+          of the robot.
+      */
+      virtual rhoban_geometry::Point linear_position( double u ) const = 0;
+
+      virtual ~Avoid_obstacle();
+};
+
+#endif

src/ai/makers/space_discretization.cpp

@@ -0,0 +1,24 @@
+#include "space_discretization.h"
+
+int Space_discretization::getX(){
+	return X;
+}
+
+int Space_discretization::getY(){
+	return Y;
+}
+
+double Space_discretization::getEX(){
+	return edgeX;
+}
+
+double Space_discretization::getEY(){
+	return edgeY;
+}
+
+void Space_discretization::print_space_discretization(){
+	cout << "X : " << Space_discretization::getX() << endl;
+	cout << "Y : " << Space_discretization::getY() << endl;
+	cout << "edgeX : " << Space_discretization::getEX() << endl;
+	cout << "edgeY : " << Space_discretization::getEY() << endl;
+}

src/ai/makers/space_discretization.h

@@ -0,0 +1,50 @@
+#ifndef __SPACE_DISCRETIZATION_H__
+#define __SPACE_DISCRETIZATION_H__
+
+#include <cstdio>
+#include <iostream>
+#include <cstring>
+#include <stdlib.h>
+#include <cmath>
+#include <list>
+
+
+
+using namespace std;
+
+class Space_discretization{
+	int X;
+	int Y;
+	double edgeX;
+	double edgeY;
+public:
+	// constructor 1
+	Space_discretization(int X, int Y, double eX, double eY):X(X), Y(Y), edgeX(eX), edgeY(eY){}
+
+	// constructor 2
+	Space_discretization(double W, double H, double edge){
+		X = (int)(W/edge);
+		Y = (int)(H/edge);
+		edgeX = W/X;
+		edgeY = H/Y;
+	}
+
+	// constructor 3
+	// We suppose that H is always <= W
+	Space_discretization(double W, double H){
+		double e = H/W;
+		X = (int)(W/e);
+		Y = (int)(H/e);
+		edgeX = W/X;
+		edgeY = H/Y;
+	}
+
+	int getX();
+	int getY();
+	double getEX();
+	double getEY();
+	void print_space_discretization();
+
+};
+
+#endif /* __SPACE_DISCRETIZATION_H__*/