Merge pull request #147 from CPFL/velocity_set_fix

Fix invalid pointer access issue
autowarefoundation · Dec 2, 2015 · 2d69187 · 2d69187
2 parents 730d038 + b44c038
commit 2d69187
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/...rc/computing/planning/motion/packages/driving_planner/nodes/velocity_set/velocity_set.cpp b/...rc/computing/planning/motion/packages/driving_planner/nodes/velocity_set/velocity_set.cpp
@@ -526,19 +526,19 @@ static void DisplayDecelerationRange(int i)
     marker.points.clear();
 }
 
-static EControl vscanDetection(int closest_waypoint)
+static EControl vscanDetection()
 {
 
-    if (_vscan.empty() == true)
+    if (_vscan.empty() == true || _closest_waypoint < 0)
         return KEEP;
 
     int decelerate_or_stop = -10000;
     int decelerate2stop_waypoints = 15;
-    for (int i = closest_waypoint; i < closest_waypoint + _search_distance; i++) {
+    for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) {
         decelerate_or_stop++;
         if (decelerate_or_stop > decelerate2stop_waypoints ||
 	    (decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) ||
-	    (decelerate_or_stop >= 0 && i == closest_waypoint+_search_distance-1))
+	    (decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1))
 	    return DECELERATE;
         if (i > _path_dk.getSize() - 1 )
             return KEEP;
@@ -629,7 +629,7 @@ static EControl ObstacleDetection()
     std::cout << "current_velocity : " << mps2kmph(_current_vel) << std::endl;
     DisplayDetectionRange(_closest_waypoint);
     DisplayDecelerationRange(_closest_waypoint);
-    EControl vscan_result = vscanDetection(_closest_waypoint);
+    EControl vscan_result = vscanDetection();
 
     if (prev_detection == KEEP) {
       if (vscan_result != KEEP) { // found obstacle