def test_acceleration_distribution(self):
"""Test how the acceleration is distributed along an distance.
Expected behavior:
The speed shall change much more in the beginning than towards the end, when measuring the change per
meter (not per second).
"""
current_speed = 0.0
target_speed = 30.0
speed_calc = SpeedCalculator(target_speed=target_speed, current_speed=current_speed,
target_acceleration=0.0, current_accleration=0.0,
acceleration_limit=10.0, jerk_limit=10.0)
distances = np.arange(0.0, 100.0, 1.0)
speeds = [speed_calc.get_speed_at_distance(distance) for distance in distances]
self.assertEqual(speeds[0], current_speed)
self.assertEqual(speeds[-1], target_speed)
# The speed should increase rapidly when going slow and then smooth out towards the target.
previous_diff = np.inf
diffs = []
for speed1, speed2 in zip(speeds[:-1], speeds[1:]):
diff = speed2 - speed1
diffs.append(diff)
self.assertLessEqual(diff, previous_diff)
previous_diff = diff
def test_imbalanced_acceleration(self):
"""Test non-equal current and target acceleration.
First calculate a balanced acceleration curve as reference,
then make sure that any values along that curve gives roughly the same output.
Expected behavior:
Continuing on an ongoing acceleration should produce similar result as in the initial calculation.
"""
target_speed = 30.0
distances = np.arange(0.0, 100.0, 1.0)
ref_speed_calc = SpeedCalculator(target_speed=target_speed, current_speed=0.0,
target_acceleration=0.0, current_accleration=0.0,
acceleration_limit=10.0, jerk_limit=10.0)
ref_speeds = [ref_speed_calc.get_speed_at_distance(distance) for distance in distances]
ref_accs = [ref_speed_calc.get_acceleration_at_distance(distance) for distance in distances]
# plt.figure(1), plt.plot(distances, ref_speeds, 'r')
# plt.figure(2), plt.plot(distances, ref_accs, 'r')
for i in range(len(distances)):
current_speed = ref_speeds[i]
current_acceleration = ref_accs[i]
speed_calc = SpeedCalculator(target_speed=target_speed, current_speed=current_speed,
target_acceleration=0.0, current_accleration=current_acceleration,
acceleration_limit=10.0, jerk_limit=10.0)
speeds = [speed_calc.get_speed_at_distance(distance - distances[i]) for distance in distances[i:]]
accs = [speed_calc.get_acceleration_at_distance(distance - distances[i]) for distance in distances[i:]]
np.testing.assert_array_almost_equal(speeds, ref_speeds[i:], decimal=1)
np.testing.assert_array_almost_equal(accs, ref_accs[i:], decimal=1)
def __accelerate_to_speed_limit(self):
"""Set waypoints speed to accelerate the vehicle to max velocity."""
# Base the acceleration on the velocity from preceding waypoint.
current_speed = get_waypoint_velocity(self.preceding_waypoint)
current_acceleration = self.preceding_waypoint.acceleration
speed_calc = SpeedCalculator(
target_speed=self.target_velocity,
current_speed=current_speed,
target_acceleration=0.0,
current_accleration=current_acceleration,
acceleration_limit=self.acceleration_limit,
jerk_limit=self.jerk_limit)
distances = self.__calc_distances()
for idx in range(len(self.waypoints)):
speed = speed_calc.get_speed_at_distance(distances[idx])
set_waypoint_velocity(self.waypoints, idx, speed)
acceleration = speed_calc.get_acceleration_at_distance(
distances[idx])
self.waypoints[idx].acceleration = acceleration
def test_jerk_limit(self):
"""Test how the jerk limit affects acceleration
Expected behavior:
The acceleration up to target speed shall be faster as the jerk_limit is increased.
"""
current_speed = 0.0
target_speed = 30.0
previous_acceleration_distance = np.inf
for jerk_limit in np.arange(5.0, 20.1, 2.5):
speed_calc = SpeedCalculator(target_speed=target_speed, current_speed=current_speed,
target_acceleration=0.0, current_accleration=0.0,
acceleration_limit=np.inf, jerk_limit=jerk_limit)
speeds = [speed_calc.get_speed_at_distance(distance) for distance in np.arange(0.0, 100.0, 1.0)]
self.assertEqual(speeds[0], 0.0)
self.assertEqual(speeds[-1], 30.0)
# Check that the acceleration is faster when permitting more jerk.
acceleration_distance = speeds.index(30.0)
self.assertLess(acceleration_distance, previous_acceleration_distance)
previous_acceleration_distance = acceleration_distance
def test_acceleration_limit(self):
"""Test how the acceleration limit affects acceleration
Expected behavior:
The acceleration up to target speed shall be faster as the acceleration limit is increased, up to some
point where the limitation no longer have any effect.
"""
current_speed = 0.0
target_speed = 30.0
previous_acceleration_distance = np.inf
is_acceleration_effectively_limited = True
numFasterAccelerations = 0
numEqualAccelerations = 0
distances = np.arange(0.0, 100.0, 1.0)
for acc_limit in np.arange(5.0, 25.1, 2.5):
speed_calc = SpeedCalculator(target_speed=target_speed, current_speed=current_speed,
target_acceleration=0.0, current_accleration=0.0,
acceleration_limit=acc_limit, jerk_limit=10.0)
speeds = [speed_calc.get_speed_at_distance(distance) for distance in distances]
accs = [speed_calc.get_acceleration_at_distance(distance) for distance in distances]
self.assertEqual(speeds[0], 0.0)
self.assertEqual(speeds[-1], 30.0)
self.assertTrue(all(accs <= acc_limit))
acceleration_distance = speeds.index(30.0)
if is_acceleration_effectively_limited:
if acceleration_distance < previous_acceleration_distance:
numFasterAccelerations += 1
else:
self.assertTrue(all(accs < acc_limit))
is_acceleration_effectively_limited = False
numEqualAccelerations += 1
else:
# Acceleration is no longer reaching the limit
self.assertTrue(all(accs < acc_limit))
self.assertEqual(acceleration_distance, previous_acceleration_distance)
numEqualAccelerations += 1
previous_acceleration_distance = acceleration_distance
self.assertTrue(numFasterAccelerations > 0)
self.assertTrue(numEqualAccelerations > 0)
def __stop_at_waypoint(self, stop_idx):
""" Reduce speed to stop at stop_idx.
Parameters
----------
stop_idx : The waypoint index where the vehicle should reach a speed of zero.
"""
stop_idx += 1 # Offset for preceding waypoint
# Calculate the reverse distances from the stop_idx and backwards.
distances = [
0.0
] + self.__calc_distances() # distance to preceding waypoint is 0
distances = [distances[stop_idx] - distance for distance in distances]
# Try more and more harsh jerk_limits until finding one that decelerate to stop in time.
# When also unable to stop using the max permitted jerk limit, then deceleration is skipped.
for jerk_limit in np.arange(2.5, self.jerk_limit, 2.5):
# Create temporary lists with speed and acceleration to be modified below.
speeds = [get_waypoint_velocity(self.preceding_waypoint)] + \
[get_waypoint_velocity(wp) for wp in self.waypoints]
accs = [self.preceding_waypoint.acceleration] + \
[wp.acceleration for wp in self.waypoints]
# Set the speed and acceleration after the stop_idx to zero.
speeds[stop_idx + 1:] = [0.0] * (len(speeds) - stop_idx - 1)
accs[stop_idx + 1:] = [0.0] * (len(accs) - stop_idx - 1)
# Calculate the deceleration backwards from the stop_idx,
# until reaching a speed that is greater than what was already requested.
speed_calc = SpeedCalculator(
target_speed=self.max_velocity,
current_speed=0.0,
target_acceleration=0.0,
current_accleration=0.0,
acceleration_limit=self.acceleration_limit,
jerk_limit=jerk_limit)
for idx in range(stop_idx, -1, -1):
speed = speed_calc.get_speed_at_distance(distances[idx])
acc = -speed_calc.get_acceleration_at_distance(distances[idx])
if speed > speeds[idx] or np.isclose(speed, speeds[idx]):
rospy.logdebug(
'Success: jerk_limit %s decelerates from %s m/s (>=%s m/s) at idx %s',
jerk_limit, speed, speeds[idx], idx)
stop_possible = True
break
speeds[idx] = speed
accs[idx] = acc
else:
rospy.logdebug(
'Failed: jerk_limit %s only decelerates from %s m/s (<%s m/s) at idx %s',
jerk_limit, speed,
get_waypoint_velocity(self.preceding_waypoint), idx)
stop_possible = False
if stop_possible:
for idx in range(len(self.waypoints)):
set_waypoint_velocity(self.waypoints, idx, speeds[idx + 1])
self.waypoints[idx].acceleration = accs[idx + 1]
break
else:
rospy.loginfo('Unable to stop from %s m/s in %s m',
get_waypoint_velocity(self.preceding_waypoint),
distances[0])