Skip to content

Commit ca8f06b

Browse files
ShellyRiverShellyRiver
committed
completed simple online trajectory planning, might need parallel running, needs to clean the code
1 parent a58eecf commit ca8f06b

1 file changed

Lines changed: 227 additions & 27 deletions

File tree

‎GEMstack/onboard/planning/racing_planning.py‎

Lines changed: 227 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ def waypoint_generate(vehicle_state, cones, cone_idx):
6161
target_heading = car_heading
6262

6363
# ===== Parameters =====
64-
u_turn_radius = 11.5 # Radius for U-turn
64+
u_turn_radius = 10.0 # Radius for U-turn
6565
offset = 2.0 # Offset for left/right pass
6666
lookahead_distance = 10.0 # Distance ahead for fixed point
6767
# ======================
@@ -455,7 +455,7 @@ def no_cone_planning(vehicle_dict):
455455
vehicle_x, vehicle_y = vehicle_dict['position'][0], vehicle_dict['position'][1]
456456
vehicle_heading = vehicle_dict['heading']
457457
vehicle_velocity = vehicle_dict['velocity']
458-
step_size = 0.5 * (max(1,vehicle_velocity))
458+
step_size = 0.5
459459
for i in range(10):
460460
temp_points.append([vehicle_x + i * step_size * np.cos(vehicle_heading),
461461
vehicle_y + i * step_size * np.sin(vehicle_heading)])
@@ -485,8 +485,11 @@ def got_new_cone(current_cones, prev_cones):
485485
class SlalomTrajectoryPlanner(Component):
486486
def __init__(self, **kwargs):
487487
# You can accept args here if needed
488+
self.prev_vehicle_position = None
488489
self.trajectory = None
489490
self.prev_cones = None
491+
self.travelled_distance = 0.0
492+
self.cones = []
490493
# ----------------------------
491494
# Predifined-Cones Simulation
492495
# self.run_fake_plan = True
@@ -517,26 +520,40 @@ def update(self, agents: Dict[str, AgentState], vehicle: VehicleState):
517520
for id, agent in agents.items():
518521
if agent.type == AgentEnum.CONE:
519522
# ===== RUNNING ONBOARD =====
520-
cones.append({
521-
'id': id,
522-
'x': agent.pose.x,
523-
'y': agent.pose.y,
524-
'orientation': agent.activity
525-
})
526-
# ===== TESTING ONBOARD in BASIC SIM =====
527-
# if n % 2 == 0:
528-
# curr_activity = 'LEFT'
529-
# elif n % 2 == 1:
530-
# curr_activity = 'RIGHT'
531-
# else:
532-
# curr_activity = 'STANDING'
533523
# cones.append({
534524
# 'id': id,
535525
# 'x': agent.pose.x,
536526
# 'y': agent.pose.y,
537-
# 'orientation': curr_activity
527+
# 'orientation': agent.activity
538528
# })
539-
# n = n + 1
529+
# ===== TESTING ONBOARD in BASIC SIM =====
530+
if n > 3:
531+
break
532+
if n % 4 == 0:
533+
curr_activity = 'LEFT'
534+
elif n % 4 == 1:
535+
curr_activity = 'RIGHT'
536+
elif n % 4 == 2:
537+
curr_activity = 'LEFT'
538+
else:
539+
curr_activity = 'STANDING'
540+
c = {
541+
'id': id,
542+
'x': agent.pose.x,
543+
'y': agent.pose.y,
544+
'orientation': curr_activity
545+
}
546+
n = n + 1
547+
if c['id'] not in {cone['id'] for cone in self.cones}:
548+
self.cones.append(c)
549+
550+
curr_pos = np.array([vehicle.pose.x, vehicle.pose.y])
551+
if self.prev_vehicle_position is None:
552+
distance_increment = 0.0
553+
else:
554+
distance_increment = np.linalg.norm(curr_pos - self.prev_vehicle_position)
555+
556+
self.prev_vehicle_position = curr_pos
540557

541558
vehicle_dict = {
542559
'position': [vehicle.pose.x, vehicle.pose.y],
@@ -546,19 +563,23 @@ def update(self, agents: Dict[str, AgentState], vehicle: VehicleState):
546563
if self.DEBUG_MODE:
547564
print("===================== STATES =====================")
548565
print(f"Vehicle State: {vehicle_dict}")
549-
print(f"Detected Cones: {cones}")
566+
print(f"Detected Cones: {self.cones}")
550567
print("===================== ====== =====================")
568+
569+
print("cones when planning: ", self.cones)
570+
self.trajectory = self.online_trajectory_planning(vehicle_dict, self.cones, distance_increment)
571+
551572
# If no cones detected, drive forward
552-
if len(cones) == 0:
573+
if len(self.cones) == 0:
553574
self.trajectory = no_cone_planning(vehicle_dict)
554-
# Otherwise, plan trajectory
555-
elif got_new_cone(cones, self.prev_cones):
556-
# Replan only if new cones are detected
557-
self.trajectory = plan_full_slalom_trajectory(vehicle_dict, cones)
558-
self.prev_cones = cones
559-
else:
560-
# No need to update the plan if the same cones are detected
561-
self.prev_cones = cones
575+
# # Otherwise, plan trajectory
576+
# elif got_new_cone(cones, self.prev_cones):
577+
# # Replan only if new cones are detected
578+
# self.trajectory = plan_full_slalom_trajectory(vehicle_dict, cones)
579+
# self.prev_cones = cones
580+
# else:
581+
# # No need to update the plan if the same cones are detected
582+
# self.prev_cones = cones
562583

563584
# Testing with predefined fake generated cone positions
564585
elif self.run_fake_plan:
@@ -610,13 +631,192 @@ def update(self, agents: Dict[str, AgentState], vehicle: VehicleState):
610631
# Update output
611632
return self.trajectory
612633

634+
def online_trajectory_planning(self, vehicle_state, cones, distance_increment, replan_threshold=100.0):
635+
print("planing......")
636+
if not hasattr(self, 'prev_cones'):
637+
self.prev_cones = None
638+
639+
if not hasattr(self, 'no_cone_ahead'):
640+
self.no_cone_ahead = False
641+
642+
if not hasattr(self, 'visited_cone_ids'):
643+
self.visited_cone_ids = set()
644+
645+
stitch_idx = -1
646+
647+
def got_new_cone(current, prev):
648+
if prev is None:
649+
return True
650+
prev_ids = {c['id'] for c in prev}
651+
return any(c['id'] not in prev_ids for c in current)
652+
653+
self.travelled_distance += distance_increment
654+
new_cone_detected = got_new_cone(self.cones, self.prev_cones)
655+
656+
# Plan at the beginning or when new cones detected or after threshold distance
657+
if self.trajectory is None or new_cone_detected or not self.no_cone_ahead or True:
658+
self.prev_cones = self.cones
659+
660+
if self.trajectory is None:
661+
current_position = vehicle_state['position']
662+
init_state = {
663+
'x': current_position[0],
664+
'y': current_position[1],
665+
'psi': vehicle_state['heading'],
666+
'c': 0.0,
667+
'v': vehicle_state['velocity']
668+
}
669+
else:
670+
stitch_idx, init_point, heading = self.get_future_point_on_trajectory(self.trajectory, vehicle_state['position'], lookahead_distance=500.0)
671+
init_state = {
672+
'x': init_point[0],
673+
'y': init_point[1],
674+
'psi': heading,
675+
'c': 0.0,
676+
'v': vehicle_state['velocity']
677+
}
678+
679+
print("all cones: ", self.cones)
680+
current_cone_idx, updated_cones = self.get_current_cone_idx(self.cones, init_state)
681+
self.cones = updated_cones
682+
print("updated cones are here: ", self.cones)
683+
print("init state: ", init_state)
684+
print("current cone: ", current_cone_idx)
685+
686+
# No cone ahead
687+
if current_cone_idx == -1:
688+
self.no_cone_ahead = True
689+
return self.trajectory
690+
else:
691+
self.no_cone_ahead = False
692+
693+
# No need to plan if there is no new cone detected and no cone ahead
694+
if not new_cone_detected and self.no_cone_ahead:
695+
return self.trajectory
696+
697+
self.visited_cone_ids.add(self.cones[current_cone_idx]['id'])
698+
scenario, flex_wps, fixed_wp, target_heading = waypoint_generate(vehicle_state, self.cones, current_cone_idx)
613699

700+
if flex_wps and fixed_wp is not None:
701+
final_state = {
702+
'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': target_heading, 'c': 0.0
703+
}
614704

705+
# Stitch from current vehicle position to new plan start
706+
if self.trajectory is not None:
707+
# 1. Plan new trajectory from init_state onward
708+
x_new, y_new, _, _, v_new, _, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps)
615709

710+
# 2. Cut old trajectory up to init_state (e.g., index `stitch_idx`)
711+
old_points = self.trajectory.points[:stitch_idx]
712+
old_x = [p[0] for p in old_points]
713+
old_y = [p[1] for p in old_points]
714+
old_v = [vehicle_state['velocity']] * len(old_x) # or extract from old trajectory if available
616715

716+
# 3. Combine old + new
717+
x_full = np.concatenate([old_x, x_new])
718+
y_full = np.concatenate([old_y, y_new])
719+
v_full = np.concatenate([old_v, v_new])
617720

721+
# 4. Create trajectory
722+
self.trajectory = to_gemstack_trajectory(x_full, y_full, v_full)
723+
else:
724+
x, y, _, _, v, _, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps)
725+
self.trajectory = to_gemstack_trajectory(x, y, v)
618726

727+
self.travelled_distance = 0.0
619728

729+
return self.trajectory
730+
731+
@staticmethod
732+
def get_future_point_on_trajectory(trajectory, vehicle_position, lookahead_distance=80.0):
733+
"""
734+
Finds a point `lookahead_distance` ahead of the current vehicle position along the trajectory.
735+
"""
736+
traj_points = trajectory.points
737+
current_pos = np.array(vehicle_position)
738+
739+
# Step 1: Find the closest point on trajectory
740+
dists = [np.linalg.norm(current_pos - np.array([p[0], p[1]])) for p in traj_points]
741+
closest_idx = np.argmin(dists)
742+
743+
# Step 2: Accumulate distance from closest_idx forward
744+
accumulated = 0.0
745+
heading = 0
746+
for i in range(closest_idx + 1, len(traj_points)):
747+
p1 = np.array([traj_points[i - 1][0], traj_points[i - 1][1]])
748+
p2 = np.array([traj_points[i][0], traj_points[i][1]])
749+
heading = np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
750+
segment = np.linalg.norm(p2 - p1)
751+
accumulated += segment
752+
if accumulated >= lookahead_distance:
753+
return i, traj_points[i], heading # Return future point
754+
755+
p1 = np.array([traj_points[-2][0], traj_points[-2][1]])
756+
p2 = np.array([traj_points[-1][0], traj_points[-1][1]])
757+
heading = np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
758+
# If not enough length, return the last point
759+
return -1, traj_points[-1], heading
760+
761+
def get_current_cone_idx(self, cones, init_state, forward_dist=50.0, angle_thresh=np.pi):
762+
"""
763+
Get the index of the nearest cone in front of the init_state.
764+
If a 'STANDING' cone is found, previous cones are flipped and returned with the index of the standing cone.
765+
766+
Args:
767+
cones: List of cones.
768+
init_state: Dict with keys 'x', 'y', 'psi'.
769+
forward_dist: Max distance to search for cones ahead.
770+
angle_thresh: Angle threshold to filter cones roughly in front.
771+
772+
Returns:
773+
idx: Index of the cone in front (after STANDING logic if needed).
774+
updated_cones: Possibly updated cones with flipped orientations.
775+
"""
776+
pos = np.array([init_state['x'], init_state['y']])
777+
heading = init_state['psi']
778+
heading_vec = np.array([np.cos(heading), np.sin(heading)])
779+
780+
best_idx = None
781+
min_dist = float('inf')
782+
783+
# Search in the list of cones, which one is nearest ahead of init_state
784+
for i, cone in enumerate(cones):
785+
if cone['id'] in self.visited_cone_ids:
786+
continue # Skip already visited cones
787+
cone_pos = np.array([cone['x'], cone['y']])
788+
vec_to_cone = cone_pos - pos
789+
dist = np.linalg.norm(vec_to_cone)
790+
if dist > forward_dist:
791+
continue
792+
angle = np.arccos(np.clip(np.dot(heading_vec, vec_to_cone / (dist + 1e-8)), -1.0, 1.0))
793+
if angle < angle_thresh and dist < min_dist:
794+
best_idx = i
795+
min_dist = dist
796+
797+
# If STANDING cone is ahead, flip previous cone directions
798+
if best_idx is not None and cones[best_idx]['orientation'] == 'STANDING':
799+
updated_cones = cones[:best_idx + 1] + [
800+
self.flip_cone_orientation(c) for c in cones[:best_idx][::-1]
801+
] + cones[best_idx + 1:]
802+
print("updated cones: ", updated_cones)
803+
return best_idx, updated_cones
804+
else:
805+
# -1 means no available cone is ahead
806+
return best_idx if best_idx is not None else -1, cones
807+
808+
@staticmethod
809+
def flip_cone_orientation(cone):
810+
"""
811+
Flip cone orientation LEFT↔RIGHT
812+
"""
813+
flipped = cone.copy()
814+
flipped['id'] = cone['id'] + 'flipped'
815+
if cone['orientation'] == 'LEFT':
816+
flipped['orientation'] = 'RIGHT'
817+
elif cone['orientation'] == 'RIGHT':
818+
flipped['orientation'] = 'LEFT'
819+
return flipped
620820

621821
########################################################################################################################
622822
########################################################################################################################

0 commit comments

Comments
 (0)