From: ehiker <ehiker@ehikers-lubuntu.(none)>
Date: Wed, 19 Oct 2011 12:02:36 +0000 (+0200)
Subject: robotfsm, pathplan: add angle for pathplan start point
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/17c3d9cc4ac5802bcb2a3c18c85c72f82eb10352

robotfsm, pathplan: add angle for pathplan start point
---

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 1daf3428..e4b3b7f7 100644
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -227,7 +227,7 @@ void GraphCell2XY(GraphMapCell *c, int *x, int *y)
 
 
 
-int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
+int aAlgorithm_shape(double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, double start_angle, double goal_angle, GraphMapCell **original_path)
 {
 	/**
 	 * prioritySet : Set of nodes to process order by heuristic distance
diff --git a/src/pathplan/aalgorithm.h b/src/pathplan/aalgorithm.h
index 062e5624..e8fb9c3e 100644
--- a/src/pathplan/aalgorithm.h
+++ b/src/pathplan/aalgorithm.h
@@ -57,7 +57,7 @@ void GraphCell2XY(GraphMapCell *c, int *x, int *y);
 
 int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path);
 
-int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path);
+int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, double start_angle, double goal_angle, GraphMapCell **original_path);
 
 #ifdef TESTBITMAPS
 void init_bitmap(bitmap_dim bitmap[], int bitmap_ab, int bitmap_af, int bitmap_w2);
diff --git a/src/pathplan/path_planner.c b/src/pathplan/path_planner.c
index 7e43ebf9..ef7c33ae 100644
--- a/src/pathplan/path_planner.c
+++ b/src/pathplan/path_planner.c
@@ -71,7 +71,7 @@ static void add_safety_margin()
  */
 
 
-int path_planner(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real , PathPoint ** simple_path, double * angle, enum _astar_method astar_method)
+int path_planner(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real , PathPoint ** simple_path, double * start_angle, double * goal_angle, enum _astar_method astar_method)
 {
 	int nbpoints, count;
         int ret;
@@ -104,7 +104,7 @@ int path_planner(double xstart_real,double ystart_real, double xgoal_real, doubl
 	      nbpoints = aAlgorithm(xstart_real,ystart_real, xgoal_real, ygoal_real, &astar_path);
 	}
 	else{
-	      nbpoints = aAlgorithm_shape(xstart_real,ystart_real, xgoal_real, ygoal_real, &astar_path);
+	      nbpoints = aAlgorithm_shape(xstart_real,ystart_real, xgoal_real, ygoal_real, *start_angle, *goal_angle, &astar_path);
 	}
 	
 	if (nbpoints < 0) {
@@ -135,7 +135,7 @@ int path_planner(double xstart_real,double ystart_real, double xgoal_real, doubl
 	// DBG("Simplifing the path\n");
 	// Calc a simplest path 
 	(*simple_path) = (PathPoint *) malloc(sizeof(PathPoint));
-	count = path_simplifier(original_path, nbpoints, *simple_path, angle);
+	count = path_simplifier(original_path, nbpoints, *simple_path, goal_angle);
 	// Do not send the first point
 	tmp=(*simple_path);
 	(*simple_path)= (*simple_path)->next;
diff --git a/src/pathplan/path_planner.h b/src/pathplan/path_planner.h
index 744eb474..7d2ce7ff 100644
--- a/src/pathplan/path_planner.h
+++ b/src/pathplan/path_planner.h
@@ -76,7 +76,7 @@ ASTAR_METHOD_SHAPE
 extern "C" {
 #endif 
 		
-	int path_planner(double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, PathPoint** simple_path, double* angle, enum _astar_method astar_method);
+	int path_planner(double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, PathPoint** simple_path, double* start_angle , double* goal_angle, enum _astar_method astar_method);
 		
 #ifdef __cplusplus
 }
diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index a039e5a2..6e95b62b 100644
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -108,7 +108,7 @@ static enum target_status new_goal(struct move_target *move_target, double start
 	// Where to start trajectory planning?
 	get_future_pos(start_in_future, future_traj_point, time_ts);
 	Point start(future_traj_point.x, future_traj_point.y);
-	
+	double start_angle = future_traj_point.phi;
 	Trajectory *t = new Trajectory(move_target->tc, backward);
 
 	/*
@@ -126,7 +126,7 @@ static enum target_status new_goal(struct move_target *move_target, double start
 
 	if (move_target->use_planning) {
 		/// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
-		switch (path_planner(start.x, start.y, target.x, target.y, &path, &angle, ASTAR_METHOD_SHAPE)) {
+		switch (path_planner(start.x, start.y, target.x, target.y, &path, &start_angle, &angle, ASTAR_METHOD_SHAPE)) {
 			case PP_ERROR_MAP_NOT_INIT:
 				ret = TARGET_ERROR; break;
 			case PP_ERROR_GOAL_IS_OBSTACLE: