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},
{"CarrotLatControl", PERSISTENT},
{"LatMpcPathCost", PERSISTENT},
{"LatMpcPathCostTurn", PERSISTENT },
{"LatMpcMotionCost", PERSISTENT},
{"LatMpcMotionCost2", PERSISTENT},
{"LatMpcMotionCostTurn", PERSISTENT},
{"LatMpcAccelCost", PERSISTENT},
{"LatMpcJerkCost", PERSISTENT},
{"LatMpcSteeringRateCost", PERSISTENT},

19
selfdrive/carrot_settings.json
View File

@ -131,6 +131,19 @@
"default": 1,
"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": "조향튜닝",
"name": "LatMpcMotionCost",
@ -146,11 +159,11 @@
},
{
"group": "조향튜닝",
"name": "LatMpcMotionCost2",
"title": "*LATMPC.MotionCost2(11)",
"name": "LatMpcMotionCostTurn",
"title": "*LATMPC.MotionCostTurn(1)",
"descr": "헤딩가중치",
"egroup": "LAT",
"etitle": "*LATMPC.MotionCost2(11)",
"etitle": "*LATMPC.MotionCostTurn(1)",
"edescr": "",
"min": 0,
"max": 500,

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

@ -81,15 +81,16 @@ class LateralPlanner:
self.lat_mpc.reset(x0=self.x0)
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
if self.readParams <= 0:
self.readParams = 100
self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply")
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.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_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01
STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost")
@ -138,15 +139,15 @@ class LateralPlanner:
self.LP.lane_change_multiplier = 1.0
lateral_motion_cost = self.lateralMotionCost
path_cost = PATH_COST
path_cost = self.lateralPathCost
atc_type = sm['carrotMan'].atcType
if atc_activate:
if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1):
lateral_motion_cost = self.lateralMotionCost2
#path_cost *= 2
lateral_motion_cost = self.lateralMotionCostTurn
path_cost = self.lateralPathCostTurn
elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1):
lateral_motion_cost = self.lateralMotionCost2
#path_cost *= 2
lateral_motion_cost = self.lateralMotionCostTurn
path_cost = self.lateralPathCostTurn
# lanelines calculation?
self.LP.lanefull_mode = self.useLaneLineMode

3
system/manager/manager.py
View File

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