Merge pull request #70 from open-rdc/add_#69

Add #69
open-rdc · Nov 3, 2017 · f6b5f52 · f6b5f52
2 parents 11e289e + df4b4c9
commit f6b5f52
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/fulanghua_waypoints_nav/src/waypoints_nav.cpp b/fulanghua_waypoints_nav/src/waypoints_nav.cpp
@@ -363,6 +363,7 @@ class WaypointsNavigation{
                     }
 
                     startNavigationGL(*current_waypoint_);
+                    int resend_goal = 0;
                     double start_nav_time = ros::Time::now().toSec();
                     while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                         if(!has_activate_)
@@ -374,6 +375,12 @@ class WaypointsNavigation{
                             std_srvs::Empty empty;
                             clear_costmaps_srv_.call(empty);
                             startNavigationGL(*current_waypoint_);
+                            resend_goal++;
+                            if(resend_goal == 3) {
+                                ROS_WARN("Skip waypoint.");
+                                current_waypoint_++;
+                                startNavigationGL(*current_waypoint_);
+                            }
                             start_nav_time = time;
                         }
                         sleep();