Merge branch 'A4_planning_part1' into A4_planning_patrick

Author: udymd

GEMstack/onboard/planning/longitudinal_planning.py

@@ -123,43 +123,277 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m
 
     #TODO: actually do something to points and times
     points = [p for p in path_normalized.points]
-
+    times = [t for t in path_normalized.times]
     #=============================================
 
     print("-----LONGITUDINAL PLAN-----")
-
-    look_ahead_time = 10.0
-    dt = 0.2
-    stop_distance_buffer = 0.2
-
-    trajectory_length = (look_ahead_time / dt) + 1
-    times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist()
-
-    new_points = [points[0]]
-
-    for _ in range(1, len(times)):
-        distance_to_stop = current_speed **2 / (2 * deceleration)
-        if new_points[-1][0] >= points[-1][0]:
-            new_points.append([new_points[-1][0], 0])
-        elif (distance_to_stop + stop_distance_buffer) >= (points[-1][0] - new_points[-1][0]):
-            # start decelerating
-            new_points.append([new_points[-1][0] + current_speed * dt - 0.5 * deceleration * dt**2, 0])
-            current_speed -= deceleration * dt
+    print("path length: ", path.length())
+    length = path.length()
+
+    # If the path is too short, just return the path for preventing sudden halt of simulation
+    if length < 0.05:
+        return Trajectory(path.frame, points, times)
+
+    # This assumes that the time denomination cannot be changed
+
+    # Starting point
+    x0 = points[0][0]
+    cur_point = points[0]
+    cur_time = times[0]
+    cur_index = 0
+
+    new_points = []
+    new_times = []
+    velocities = [] # for graphing and debugging purposes
+
+    while current_speed > 0 or cur_index == 0:
+        # we want to iterate through all the points and add them
+        # to the new points. However, we also want to add "critical points"
+        # where we reach top speed, begin decelerating, and stop
+        new_points.append(cur_point)
+        new_times.append(cur_time)
+        velocities.append(current_speed)
+        print("=====================================")
+        # print("new points: ", new_points)
+        # print("current index: ", cur_index)
+        # print("current speed: ", current_speed)
+        # print("current position: ", cur_point)
+
+        # Information we will need: 
+            # Calculate how much time it would take to stop
+            # Calculate how much distance it would take to stop
+        min_delta_t_stop = current_speed/deceleration
+        min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2
+        # print(min_delta_x_stop)
+        assert min_delta_x_stop >= 0
+
+
+        # Check if we are done
+
+        # If we cannot stop before or stop exactly at the final position requested
+        if cur_point[0] + min_delta_x_stop >= points[-1][0]:
+            print("In case one")
+            # put on the breaks
+
+            # Calculate the next point in a special manner because of too-little time to stop 
+            if cur_index == len(points)-1:
+                # the next point in this instance would be when we stop
+                next_point = (cur_point[0] + min_delta_x_stop, 0)
+            else:
+                next_point = points[cur_index+1]
+
+            # keep breaking until the next milestone in path
+            if next_point[0] <= points[-1][0]:
+                # print("continuing to next point")
+                delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration)
+                cur_time += delta_t_to_next_x
+                cur_point = next_point
+                current_speed -= deceleration*delta_t_to_next_x
+                cur_index += 1
+            else:
+                # continue to the point in which we would stop (current_velocity = 0)
+                # update to the next point 
+                delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration)
+                cur_point = next_point
+                cur_time += delta_t_to_next_x
+                # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero
+                #current_speed -= delta_t_to_next_x*deceleration
+                current_speed = 0
+                assert current_speed == 0
+
+        # This is the case where we are accelerating to max speed
+        # because the first if-statement covers for when we decelerating,
+        # the only time current_speed < max_speed is when we are accelerating 
         elif current_speed < max_speed:
-            # start accelerating
-            new_points.append([new_points[-1][0] + current_speed * dt + 0.5 * acceleration * dt**2, 0])
-            current_speed += acceleration * dt
+            # print("In case two")
+            # next point 
+            next_point = points[cur_index+1]
+            # accelerate to max speed
+
+            # calculate the time it would take to reach max speed
+            delta_t_to_max_speed = (max_speed - current_speed)/acceleration
+            # calculate the distance it would take to reach max speed
+            delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2
+            
+            delta_t_to_stop_from_max_speed = max_speed/deceleration
+            delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2
+            
+            delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration)
+            velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration
+            time_to_stop_from_next_point = velocity_at_next_point/deceleration
+            delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2
+            # if we would reach max speed after the next point, 
+            # just move to the next point and update the current speed and time
+            if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \
+                cur_point[0] + delta_x_to_max_speed >= next_point[0]:
+                # ("go to next point")
+                # accelerate to max speed
+                delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration)
+                cur_time += delta_t_to_next_x
+                cur_point = [next_point[0], 0]
+                current_speed += delta_t_to_next_x*acceleration
+                cur_index += 1
+                
+            # This is the case where we would need to start breaking before reaching 
+            # top speed and before the next point (i.e. triangle shape velocity)
+            elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]:
+                # print(delta_x_to_max_speed)
+                # print(delta_x_to_stop_from_max_speed)
+                # Add a new point at the point where we should start breaking 
+                # print("Adding new point to start breaking")
+                delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration)
+                # print(delta_t_to_next_x)
+                #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration)
+                next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2
+                cur_time += delta_t_to_next_x
+                cur_point = [next_x, 0]
+                current_speed += delta_t_to_next_x*acceleration
+            
+            # this is the case where we would reach max speed before the next point
+            # we need to create a new point where we would reach max speed 
+            else:
+                # print("adding new point")
+                # we would need to add a new point at max speed
+                cur_time += delta_t_to_max_speed
+                cur_point = [cur_point[0] + delta_x_to_max_speed, 0]
+                current_speed = max_speed
+
+        # This is the case where we are at max speed
+        # special functionality is that this block must 
+        # add a point where we would need to start declerating to reach
+        # the final point
+        elif current_speed == max_speed:
+            next_point = points[cur_index+1]
+            # continue on with max speed
+            # print("In case three")
+            
+            # add point to start decelerating
+            if next_point[0] + min_delta_x_stop >= points[-1][0]:
+                # print("Adding new point to start decelerating")
+                cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed
+                cur_point = [points[-1][0] - min_delta_x_stop,0]
+                current_speed = max_speed
+            else:
+                # Continue on to next point
+                # print("Continuing on to next point")
+                cur_time += (next_point[0] - cur_point[0])/current_speed
+                cur_point = next_point
+                cur_index += 1
+        
+        # This is an edge case and should only be reach 
+        # if the initial speed is greater than the max speed
+        elif current_speed > max_speed:
+            # We need to hit the breaks 
+
+            next_point = points[cur_index+1]
+            # print("In case four")
+            # slow down to max speed 
+            delta_t_to_max_speed = (current_speed - max_speed)/deceleration
+            delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2
+
+            # If we would reach the next point before slowing down to max speed
+            # keep going until we reach the next point
+            if cur_point[0] + delta_x_to_max_speed >= next_point[0]:
+                delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration)
+                cur_time += delta_t_to_next_x
+                cur_point = [next_point[0], 0]
+                current_speed -= delta_t_to_next_x*deceleration
+                cur_index += 1
+            else:
+            # We would reach max speed before the next point
+            # we need to add a new point at the point where we
+            # would reach max speed
+                cur_time += delta_t_to_max_speed
+                cur_point = [cur_point[0] + delta_x_to_max_speed, 0]
+                current_speed = max_speed
+
         else:
-            new_points.append([new_points[-1][0] + current_speed * dt, 0])
-    
+            # not sure what falls here
+            raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here")
+        
+    new_points.append(cur_point)
+    new_times.append(cur_time)
+    velocities.append(current_speed)
+        
     points = new_points
+    times = new_times
+    print("[PLAN] Computed points:", points)
+    print("[TIME] Computed time:", times)
+    print("[Velocities] Computed velocities:", velocities)
 
-    # for p, t in zip(points, times):
-    #     print(f"Time: {t:.2f}, Point: {p}")
+    #=============================================
 
-    trajectory = Trajectory(path.frame,points,times)
+    trajectory = Trajectory(path.frame,points,times,velocities)
     return trajectory
 
+def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float:
+    """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1
+    with constant acceleration a. I am assuming that we will always have a solution by settings
+    discriminant equal to zero, i'm not sure if this is an issue."""
+
+    """Consider changing the system to use linear operators instead of explicitly calculating because of instances here"""
+    # x1 = x0 + v0*t + 0.5*a*t^2
+    # x1 - x0 = v0*t + 0.5*a*t^2
+    # 0.5*a*t^2 + v0*t + x0 - x1 = 0
+    # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a)
+    # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a
+    # print("x0: ", x0)
+    # print("x1: ", x1)
+    # print("v: ", v)
+    # print("a: ", a)
+
+    t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a
+    t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a
+
+    # print("t1: ", t1)
+    # print("t2: ", t2)
+
+    if math.isnan(t1): t1 = 0
+    if math.isnan(t2): t2 = 0
+
+    # print("t1: ", t1)
+    # print("t2: ", t2)
+
+    valid_times = [n for n in [t1, t2] if n > 0]
+    if valid_times:
+        return min(valid_times)
+    else:
+        return 0.0
+
+def compute_time_triangle(x0 : float, xf: float,  v0: float, vf : float, acceleration : float, deceleration : float) -> float:
+    """
+    Compute the time to go from current point assuming we are accelerating to the point at which
+    we would need to start breaking in order to reach the final point with velocity 0."""
+    # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2
+    # 0 = v1 - d*t2 = v0 + a*t1 - d * t2
+    # t1 = (d*t2 - v0)/a
+    # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2
+    # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2
+    # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf
+    roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration,
+                      v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration,
+                      x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration)
+    print(roots)
+    t1 = max(roots)
+    assert t1 > 0
+    return t1
+
+def quad_root(a : float, b : float, c : float) -> float:
+    x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a)
+    x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a)
+
+    # print("t1: ", t1)
+    # print("t2: ", t2)
+
+    if math.isnan(x1): x1 = 0
+    if math.isnan(x2): x2 = 0
+
+    # print("t1: ", t1)
+    # print("t2: ", t2)
+
+    valid_roots = [n for n in [x1, x2] if not type(n) is complex]
+    # print(f"Valid roots {valid_roots}")
+    return valid_roots
 
 def longitudinal_brake(path : Path, deceleration : float, current_speed : float) -> Trajectory:
     """Generates a longitudinal trajectory for braking along a path."""

GEMstack/state/trajectory.py

@@ -169,6 +169,7 @@ def trim(self, start : float, end : float) -> Path:
 class Trajectory(Path):
     """A timed, piecewise linear path."""
     times : List[float]
+    velocities : Optional[List[float]] = None 
 
     def domain(self) -> Tuple[float,float]:
         """Returns the time parameter domain"""

launch/pedestrian_detection.yaml

@@ -97,3 +97,5 @@ variants:
                     # agent_detection : pedestrian_detection.FakePedestrianDetector2D
                     agent_detection : OmniscientAgentDetector  #this option reads agents from the simulator
                     state_estimation : OmniscientStateEstimator
+                    agent_detection : OmniscientAgentDetector
+