Detected when the robot passes through the goal but farther than the xy...

Detected when the robot passes through the goal but farther than the xy tolerance: Used to skip to the next goal (particular solution).

src/ackermann_planner_ros.cpp

@@ -273,7 +273,17 @@ bool AckermannPlannerROS::isGoalReached(void)
     else if(this->stuck)
       return true;
     else
-      return false;
+    {
+      double yaw=tf::getYaw(current_pose_.pose.orientation);
+      if(yaw<0.0)
+        yaw+=3.14159*2.0;
+      if(forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))<0.0)
+        return true;
+      else if(!forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))>0.0)
+        return true;
+      else
+        return false;
+    }
   } 
   else
     return false;
@@ -314,7 +324,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   static int stuck_count=0;
   static int replan_count=0;
   static bool new_segment=false;
-  bool forward;
+  bool forward,goal_passed,goal_reached;
 
   // dispatches to either ackermann sampling control or stop and rotate control, depending on whether we have been close enough to goal
   if ( ! costmap_ros_->getRobotPose(current_pose_)) 
@@ -361,15 +371,28 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   // update plan in ackermann_planner even if we just stop and rotate, to allow checkTrajectory
   dp_->update_plan_and_local_costs(current_pose_, transformed_plan,forward);
 
+  double yaw=tf::getYaw(current_pose_.pose.orientation);
+  if(yaw<0.0)
+    yaw+=3.14159*2.0;
+  if(forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))<0.0)
+    goal_passed=true;
+  else if(!forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))>0.0)
+    goal_passed=true;
+  else
+    goal_passed=false;
   odom_helper_.get_odom(odom);
-  if(this->stuck || base_local_planner::isGoalReached(*tf_,
-                                                        transformed_plan,
-                                                        *costmap_ros_->getCostmap(),
-                                                        costmap_ros_->getGlobalFrameID(),
-                                                        current_pose_,
-                                                        odom,
-                                                        current_config_.rot_stopped_vel,current_config_.trans_stopped_vel,
-                                                        current_config_.xy_goal_tolerance,current_config_.yaw_goal_tolerance))
+  if(base_local_planner::isGoalReached(*tf_,
+                                       transformed_plan,
+                                       *costmap_ros_->getCostmap(),
+                                       costmap_ros_->getGlobalFrameID(),
+                                       current_pose_,
+                                       odom,
+                                       current_config_.rot_stopped_vel,current_config_.trans_stopped_vel,
+                                       current_config_.xy_goal_tolerance,current_config_.yaw_goal_tolerance))
+    goal_reached=true;
+  else
+    goal_reached=false;
+  if(this->stuck || goal_reached || goal_passed)
   {
     if(planner_util_.set_next_path())
     {