mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 13:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove zorrobyte's curvature learner, it decreases turning ability

Browse Source
This commit is contained in:
dragonpilot
2019-10-06 20:15:21 +10:00
parent f875296a4a
commit ef4b2d32d2
2 changed files with 1 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/curvature_learner.py
-64
View File
@@ -1,64 +0,0 @@
from numpy import clip
import pickle
import csv
import os
# HOW TO
# import this module to where you want to use it, such as from ```selfdrive.controls.lib.curvature_learner import CurvatureLearner```
# create the object ```self.curvature_offset = CurvatureLearner(debug=False)```
# call the update method ```self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly)```
# The learned curvature offsets will save and load automatically
# If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch
# by Zorrobyte
# version 4
class CurvatureLearner:
def __init__(self, debug=False):
self.offset = 0.
self.learning_rate = 12000
self.frame = 0
self.debug = debug
try:
self.learned_offsets = pickle.load(open("/data/curvaturev4.p", "rb"))
except (OSError, IOError):
self.learned_offsets = {
"center": 0.,
"inner": 0.,
"outer": 0.
}
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
os.chmod("/data/curvaturev4.p", 0o777)
def update(self, angle_steers=0., d_poly=None, v_ego=0.):
if angle_steers > 0.1:
if abs(angle_steers) < 2.:
self.learned_offsets["center"] -= d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["center"]
elif 2. < abs(angle_steers) < 5.:
self.learned_offsets["inner"] -= d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["inner"]
elif abs(angle_steers) > 5.:
self.learned_offsets["outer"] -= d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["outer"]
elif angle_steers < -0.1:
if abs(angle_steers) < 2.:
self.learned_offsets["center"] += d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["center"]
elif 2. < abs(angle_steers) < 5.:
self.learned_offsets["inner"] += d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["inner"]
elif abs(angle_steers) > 5.:
self.learned_offsets["outer"] += d_poly[3] / self.learning_rate
self.offset = self.learned_offsets["outer"]
self.offset = clip(self.offset, -0.3, 0.3)
self.frame += 1
if self.frame == 12000: # every 2 mins
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
self.frame = 0
if self.debug:
with open('/data/curvdebug.csv', 'a') as csv_file:
csv_file_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
csv_file_writer.writerow([self.learned_offsets, v_ego])
return self.offset
selfdrive/controls/lib/pathplanner.py
+1 -9
View File
@@ -9,7 +9,6 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lane_planner import LanePlanner
import selfdrive.messaging as messaging
from selfdrive.controls.lib.curvature_learner import CurvatureLearner
LOG_MPC = os.environ.get('LOG_MPC', False)
@@ -30,7 +29,6 @@ class PathPlanner(object):
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
self.curvature_offset = CurvatureLearner(debug=False)
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
@@ -60,13 +58,7 @@ class PathPlanner(object):
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
#curvature_factor = VM.curvature_factor(v_ego)
if active:
curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
else:
curvfac = 0.
curvature_factor = VM.curvature_factor(v_ego) + curvfac
curvature_factor = VM.curvature_factor(v_ego)
# TODO: Check for active, override, and saturation
# if active: