Lower decel rate in get_min_stopping_distance (#109)

* Lower decel rate in get_min_stopping_distance

Lower the decel rate for desired_jerk calcs
using a variable amount based on current acceleration
and extend the search increment to minimum of 2.0 or 2.5% of
current estimate.

* Increase max_desired_jerk to 0.75

Add guard in get_max_accel to prevent sending small accel value to
get_accel_distance

* Change default velocity to be limited to 0.96*Max_velocity

Also set number of waypoints to 40 by default

* Fix dyn_default_velocity setting

Fix bug displaying max_velocity

ros/src/waypoint_updater/cfg/DynReconf.cfg

@@ -11,9 +11,9 @@ gen.add("dyn_test_stoplight_wp",   int_t, 0, "TESTING next stopline wp",
 gen.add("dyn_update_rate",         int_t, 0, "rate (Hz) at which final waypoints published",
         30, 1, 50)
 gen.add("dyn_lookahead_wps",       int_t, 0, "number of final waypoints to send",
-        20, 10,  100)
+        40, 10,  100)
 gen.add("dyn_default_velocity", double_t, 0, "top speed to use (mps)",
-        10.7, 0.0, 100.0)
+        17.0, 0.0, 100.0)
 gen.add("dyn_default_accel",    double_t, 0, "accel/decel rate (m/s) to use",
         0.8, 0.5, 5.0)
 gen.add("dyn_tl_buffer",        double_t, 0, "buffer distance (m) to stop at before traffic light wp",

ros/src/waypoint_updater/launch/waypoint_updater.launch

@@ -7,7 +7,7 @@
         <param name="handoff_velocity" type="double" value="1.0" />
         <param name="min_moving_velocity" type="double" value="1.0" />
         <param name="max_accel" type="double" value="5.0" />
-        <param name="max_desired_jerk" type="double" value="0.5" />
+        <param name="max_desired_jerk" type="double" value="0.75" />
         <param name="abs_max_jerk" type="double" value="5.0" />
     </node>
 </launch>

ros/src/waypoint_updater/waypnt_updater.py

@@ -191,13 +191,16 @@ def dyn_vars_cb(self, config, level):
                           .format(old_default_velocity,
                                   config['dyn_default_velocity']))
 
-            if config['dyn_default_velocity'] > self.max_velocity:
+            if config['dyn_default_velocity'] > 0.96 * self.max_velocity:
                 rospy.logwarn("waypoint_updater:dyn_vars_cb default_velocity "
-                              "limited to max_velocity {:3.2f}m/s"
-                              .format(self.max_velocity))
-                self.default_velocity = self.max_velocity * 0.975
+                              "limited to 0.96 * max_velocity = {:3.2f}m/s"
+                              .format(self.max_velocity*0.96))
+                self.default_velocity = self.max_velocity * 0.96
             else:
                 self.default_velocity = config['dyn_default_velocity']
+                rospy.logwarn("waypoint_updater:dyn_vars_cb default_velocity "
+                              "set to {:3.2f}m/s"
+                              .format(self.default_velocity))
 
         if old_default_accel != config['dyn_default_accel']:
             rospy.logwarn("waypoint_updater:dyn_vars_cb Adjusting default_"
@@ -388,7 +391,10 @@ def get_min_stopping_distance(self, ptr_id, max_neg_jerk):
 
         if math.fabs(-self.max_desired_jerk - max_neg_jerk) < 0.1:
             # if we are using_max_desired_jerk then start here
-            decel_rate = 0.6
+            if a > 1.1:
+                decel_rate = 0.25
+            else:
+                decel_rate = 0.4
         else:
             # empirical settings to start closer to goal
             if a < 1.0:
@@ -430,7 +436,10 @@ def get_min_stopping_distance(self, ptr_id, max_neg_jerk):
         else:
             dist_diff = -1.0
         if math.fabs(max_neg_jerk + self.max_desired_jerk) < 0.1:
-            dist_diff = 2 * dist_diff
+            if too_short is True:
+                dist_diff = max(2 * dist_diff, a_dist *.025)
+            else:
+                dist_diff = min(2 * dist_diff, -a_dist *.025)
 
         optimized = False
         counter = 0
@@ -1078,7 +1087,7 @@ def get_max_accel(self, ptr_id):
             old_dist = a_dist
 
             accel_rate = accel_rate + acc_diff
-            if accel_rate < 0.0:
+            if accel_rate < 0.1: # prevent sending really small value to get_accel_distance
                 final_accel = accel_rate - acc_diff
                 duration = rospy.get_time() - timer_start
                 rospy.logdebug("greatest accel_rate of {:3.2f} to accelerate with max_jerk"
@@ -1297,7 +1306,7 @@ def send_waypoints(self):
 
         if self.got_to_end is False:
             self.final_waypoints_start_ptr = self.closest_waypoint()
-
+        
         # do this at start of each cycle so that it doesn't change
         # if traffic cb happens in middle of loop
         if self.next_tl_wp_tmp >= self.final_waypoints_start_ptr: