brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Calculate average desired curvature using planned distance

Browse Source 
Added toggle to calculate average desired curvature using planned distance instead of the car's speed.

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 8baf713117
commit b4550a13ba
8 changed files with 35 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
selfdrive/controls/lib/lateral_planner.py
View File

@@ -35,7 +35,10 @@ class LateralPlanner:
if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
if frogpilot_planner.average_desired_curvature:
car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car)
else:
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])