robotfsm, pathplan: add angle for pathplan start point

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 1daf34285d0e5482f6e50c6a450c22b3375706d7..e4b3b7f775eb65ee15b487c4f69afc91f479c0d6 100644 (file)
--- 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 062e56241ad2be204028b9ec56e7c1695405a527..e8fb9c3e2b7c0198a1b98eeff6cd6d499c3e9735 100644 (file)
--- 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 7e43ebf9a248724967877000a01ca0e9aa079ebd..ef7c33aed1c773a15688d30dba01e3f1e0984fe6 100644 (file)
--- 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 744eb474fc8d51ba0c4ea093306af60fa306c950..7d2ce7ff63317a552448002df5a2d24085479667 100644 (file)
--- 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 a039e5a25acbb234f66c5d8657fbd444e0dd21d9..6e95b62b781f2e7ffc87b5751b1b4d6454665568 100644 (file)
--- 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: