zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

pathCostTurn, motionCostTurn

Browse Source
This commit is contained in:
ajouatom 2025-02-12 16:37:56 +09:00
parent c01206d040
commit 79e126f7dd
4 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
common/params.cc
View File

@ -311,8 +311,9 @@ std::unordered_map<std::string, uint32_t> keys = {
{"SoftHoldMode", PERSISTENT}, {"SoftHoldMode", PERSISTENT},
{"CarrotLatControl", PERSISTENT}, {"CarrotLatControl", PERSISTENT},
{"LatMpcPathCost", PERSISTENT}, {"LatMpcPathCost", PERSISTENT},
{"LatMpcPathCostTurn", PERSISTENT },
{"LatMpcMotionCost", PERSISTENT}, {"LatMpcMotionCost", PERSISTENT},
{"LatMpcMotionCost2", PERSISTENT}, {"LatMpcMotionCostTurn", PERSISTENT},
{"LatMpcAccelCost", PERSISTENT}, {"LatMpcAccelCost", PERSISTENT},
{"LatMpcJerkCost", PERSISTENT}, {"LatMpcJerkCost", PERSISTENT},
{"LatMpcSteeringRateCost", PERSISTENT}, {"LatMpcSteeringRateCost", PERSISTENT},

19
selfdrive/carrot_settings.json
View File

@ -131,6 +131,19 @@
"default": 1, "default": 1,
"unit": 10 "unit": 10
}, },
{
"group": "조향튜닝",
"name": "LatMpcPathCostTurn",
"title": "*LATMPC.PathCostTurn(200)",
"descr": "경로추종가중치, 높으면 경로우선시함.",
"egroup": "LAT",
"etitle": "*LATMPC.PathCostTurn(200)",
"edescr": "",
"min": 0,
"max": 500,
"default": 1,
"unit": 10
},
{ {
"group": "조향튜닝", "group": "조향튜닝",
"name": "LatMpcMotionCost", "name": "LatMpcMotionCost",
@ -146,11 +159,11 @@
}, },
{ {
"group": "조향튜닝", "group": "조향튜닝",
"name": "LatMpcMotionCost2", "name": "LatMpcMotionCostTurn",
"title": "*LATMPC.MotionCost2(11)", "title": "*LATMPC.MotionCostTurn(1)",
"descr": "헤딩가중치", "descr": "헤딩가중치",
"egroup": "LAT", "egroup": "LAT",
"etitle": "*LATMPC.MotionCost2(11)", "etitle": "*LATMPC.MotionCostTurn(1)",
"edescr": "", "edescr": "",
"min": 0, "min": 0,
"max": 500, "max": 500,

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

@ -81,15 +81,16 @@ class LateralPlanner:
self.lat_mpc.reset(x0=self.x0) self.lat_mpc.reset(x0=self.x0)
def update(self, sm): def update(self, sm):
global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST global LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST
self.readParams -= 1 self.readParams -= 1
if self.readParams <= 0: if self.readParams <= 0:
self.readParams = 100 self.readParams = 100
self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply") self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply")
self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01 self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
PATH_COST = self.params.get_float("LatMpcPathCost") * 0.01 self.lateralPathCost = self.params.get_float("LatMpcPathCost") * 0.01
self.lateralPathCostTurn = self.params.get_float("LatMpcPathCostTurn") * 0.01
self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01 self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01
self.lateralMotionCost2 = self.params.get_float("LatMpcMotionCost2") * 0.01 self.lateralMotionCostTurn = self.params.get_float("LatMpcMotionCostTurn") * 0.01
LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01 LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01
LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01 LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01
STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost") STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost")
@ -138,15 +139,15 @@ class LateralPlanner:
self.LP.lane_change_multiplier = 1.0 self.LP.lane_change_multiplier = 1.0
lateral_motion_cost = self.lateralMotionCost lateral_motion_cost = self.lateralMotionCost
path_cost = PATH_COST path_cost = self.lateralPathCost
atc_type = sm['carrotMan'].atcType atc_type = sm['carrotMan'].atcType
if atc_activate: if atc_activate:
if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1): if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1):
lateral_motion_cost = self.lateralMotionCost2 lateral_motion_cost = self.lateralMotionCostTurn
#path_cost *= 2 path_cost = self.lateralPathCostTurn
elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1): elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1):
lateral_motion_cost = self.lateralMotionCost2 lateral_motion_cost = self.lateralMotionCostTurn
#path_cost *= 2 path_cost = self.lateralPathCostTurn
# lanelines calculation? # lanelines calculation?
self.LP.lanefull_mode = self.useLaneLineMode self.LP.lanefull_mode = self.useLaneLineMode

3
system/manager/manager.py
View File

@ -134,8 +134,9 @@ def get_default_params():
("LateralTorqueKf", "100"), ("LateralTorqueKf", "100"),
("LateralTorqueKd", "0"), ("LateralTorqueKd", "0"),
("LatMpcPathCost", "100"), ("LatMpcPathCost", "100"),
("LatMpcPathCostTurn", "200"),
("LatMpcMotionCost", "11"), ("LatMpcMotionCost", "11"),
("LatMpcMotionCost2", "11"), ("LatMpcMotionCostTurn", "1"),
("LatMpcAccelCost", "0"), ("LatMpcAccelCost", "0"),
("LatMpcJerkCost", "4"), ("LatMpcJerkCost", "4"),
("LatMpcSteeringRateCost", "700"), ("LatMpcSteeringRateCost", "700"),