To compute if a segment is backward or forward, only use the part of the...

To compute if a segment is backward or forward, only use the part of the segment inside the costmap.

src/ackermann_planner_util.cpp

@@ -97,7 +97,11 @@ bool AckermannPlannerUtil::get_goal(geometry_msgs::PoseStamped& goal_pose)
   return base_local_planner::getGoalPose(*tf_,global_plan_,global_frame_,goal_pose);
 }
 
-bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
+bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) 
+{
+  unsigned int last_index;
+  double start_x,start_y,distance,costmap_size;
+
   if(!initialized_)
   {
     ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
@@ -108,8 +112,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
   forward_segment.clear();
   subPathIndex      = 0;
   double pathLength = 0, yaw;
-  for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
-    std::cout << orig_global_plan[i].pose.position.x << "," << orig_global_plan[i].pose.position.y << "," << tf::getYaw(orig_global_plan[i].pose.orientation) << std::endl;
+
   if(orig_global_plan.size()<5)
   {
     for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
@@ -117,6 +120,10 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
   }
   else
   {
+    start_x=orig_global_plan[0].pose.position.x;
+    start_y=orig_global_plan[0].pose.position.y;
+    last_index=0;
+    costmap_size=(this->costmap_->getSizeInMetersX()+this->costmap_->getSizeInMetersY())/2.0;
     ////divide plan by manuveurs
     for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
     {
@@ -158,7 +165,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
               yaw=tf::getYaw(subPath[0].pose.orientation);
               if(yaw<0.0)
                 yaw+=3.14159*2.0;
-              if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+              if(((subPath[last_index].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[last_index].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
               {
                 std::cout << "backward segment" << std::endl;
                 this->forward_segment.push_back(false);
@@ -168,6 +175,9 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
                 std::cout << "forward segment" << std::endl;
                 this->forward_segment.push_back(true);
               }
+              start_x=subPath[subPath.size()-1].pose.position.x;
+              start_y=subPath[subPath.size()-1].pose.position.y;
+              last_index=0;
               subPathList.push_back(subPath);
             }
             else //ignored subpath, too short
@@ -178,6 +188,12 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
             pathLength=0.0;
           }
         }
+        else
+        {
+          distance=sqrt(pow(start_x-orig_global_plan[i].pose.position.x,2.0)+pow(start_y-orig_global_plan[i].pose.position.y,2.0));
+          if(distance<costmap_size)
+            last_index++;
+        }
       }
       subPath.push_back(orig_global_plan[i]);
     }
@@ -185,7 +201,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
   yaw=tf::getYaw(subPath[0].pose.orientation);
   if(yaw<0.0)
     yaw+=3.14159*2.0;
-  if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+  if(((subPath[last_index].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[last_index].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
   {
     std::cout << "backward segment" << std::endl;
     this->forward_segment.push_back(false);