# flake8: noqa
import os
import yaml
import yappi
import numpy as np
from . import local_motion_planner, velocity_profile
yappi.set_clock_type('cpu')
centerpoints_msg = {
'header': {
'seq': 675,
'stamp': {
'sec': 1639142725,
'nsec': 553108557
},
'frame_id': 'map'
},
'image_header': {
'seq': 675,
'stamp': {
'sec': 1639142725,
'nsec': 401869058
},
'frame_id': 'map'
},
'x': [
6.906800975849173,
10.694637677745424,
12.688998883806711,
14.836224328537352,
16.531284201358773,
17.985659118991848,
18.83405393624299,
19.816292891933255,
21.01922403893393,
22.282232324987177,
23.216323692755267,
24.42527830354747,
25.659334528351856,
26.770757999745456,
27.252468446053506,
27.35275430720492,
27.61646209363504,
27.28671902648294,
26.386442032323508,
25.443824172151402,
24.414627042821216,
23.258357617126133,
20.45249879825642,
19.565755050716533,
18.687431067386914,
17.607902960768463,
16.794377423566875,
15.930810040504937,
15.003185890930036,
13.910807403608551,
13.052251815125562,
11.95667951049486,
10.744937177268584,
9.927493191026874,
8.889485722474928,
8.035023346519063,
7.63461338900078,
6.992453959452976,
6.71209201517885,
6.393527619519922,
6.133762506083879,
5.380879367263646,
4.650896455505817,
3.4292619198641487,
2.665358435309132,
1.36611254934511,
-0.07903893875621171,
-1.643241334500904,
-3.007693862074018,
-4.299021923244689,
-4.879322009151919,
-5.954045544546433,
-6.81125276269708,
-7.151307508483944,
-8.168811349957856,
-8.727249248479918,
-9.984119714460233,
-11.436779765917173,
-12.684216125827682,
-13.852596200446524,
-15.243092824798662,
-16.645659693775226,
-18.0101660642454,
-19.582155739796853,
-20.607791630753113,
-21.942138726257674,
-23.08054139678761,
-24.357016596535942,
-25.21913697236888,
-26.17225360635816,
-26.812999515395518,
-27.51980127664146,
-28.13916015284532,
-28.42599264118386,
-28.84056499779784,
-29.118759141559813,
-29.13846616274781,
-29.045167361446136,
-29.07053766376128,
-28.885263414718708,
-28.65676757795579,
-28.59871872833802,
-27.984774677425712,
-27.43331056648789,
-26.22943546582233,
-25.47161139922247,
-23.090158440201535,
-22.216113352643745,
-21.62782193349141,
-21.59346240710549,
-20.882273167187762,
-17.60354041432055,
-16.6704059172165,
-15.854812840955493,
-14.545277309255997,
-13.323878474229833,
-11.808358471952294,
-10.360754667856668,
-8.883009411959616,
-7.358572135297204,
-5.677957429218079,
-4.1346239200433015,
-2.3537579311485537,
-0.8860683067627135,
0.9502918194693833,
2.171393248825355,
2.395739328670236
],
'y': [
0.14688962583108012,
0.2187647803720446,
0.21288480835793633,
0.15828959475177107,
0.21781618209051645,
0.48460284690544775,
1.069337130729523,
1.8075295955754036,
3.0378031808265664,
4.010294636954767,
4.620023525696343,
5.506030183676886,
5.886466619985278,
6.723409175436537,
7.865103739481055,
8.884986771796873,
10.028056133502348,
11.169587923491774,
11.904786350632808,
12.607970593448105,
13.070177498308823,
13.123164185762157,
14.597396227374688,
14.542834755690924,
14.836543426994691,
15.034824524326154,
15.669173505510463,
16.504437355438874,
17.203768110055258,
18.04227851330478,
18.05737283019774,
18.389654855937593,
18.069527373783874,
17.62990104319774,
17.055688572818926,
16.346917813311094,
15.071839900777452,
13.836837665627082,
12.683686680787641,
11.45357207111999,
10.552300629851214,
9.167259590021546,
8.385730845792562,
7.837376039476197,
7.50675122146434,
7.2248270715112906,
7.079219895000799,
6.973492187891621,
7.0976147213669165,
7.827003881983442,
8.363650099891405,
9.368801588747626,
10.620554271897046,
12.001877242331076,
13.58897051578731,
13.975416356041055,
15.259513316071752,
16.53024585966339,
16.739922205564497,
17.05320762157692,
17.190395968719574,
17.495859389554212,
17.77370248524465,
17.747712438719617,
17.710092004071768,
17.52914215957156,
16.958511862319988,
16.262156733275173,
14.654388277381212,
13.360254484064958,
12.13054277509991,
10.669135428703008,
9.01717932326924,
7.783231707313008,
6.078949167455168,
4.101619616922205,
2.764105284385462,
1.5241260378914216,
0.04232172763573638,
-1.169398440823979,
-2.4442213813474485,
-3.774036997365717,
-4.593090750923803,
-6.057454251146378,
-6.900247638228977,
-7.603209424805765,
-7.750511919627549,
-7.077377185910931,
-6.073697579795512,
-4.418345929480584,
-3.4356521837704626,
-2.298161628242459,
-1.6561947445425955,
-1.1438731690375,
-0.28686182060737053,
-0.1093176701320353,
0.05481960953007359,
0.12470687593172292,
0.1393536188659813,
0.1894020253507882,
0.3318386443772814,
0.39990995760140013,
0.5072297132734261,
0.5808210293051385,
0.5169563267990621,
0.39058470208313245,
0.1942627591088022
],
'left_track_width': [
2.0003970336411956,
2.010808844881437,
2.0323098781317332,
2.0603086021926504,
2.1405931174738413,
2.2423138892998344,
2.021776766564455,
1.8153122276117095,
1.8398486570155288,
1.7658135694633543,
1.8390325154681324,
2.3090533202683154,
2.3231793200220157,
2.4752208254648176,
2.605902812718002,
2.811741312641992,
2.8567656604997183,
2.6091391647125564,
2.5138551379515044,
2.526075871326685,
2.7525240991289257,
3.131353449465531,
1.5535489548812491,
1.644131760832798,
1.5330470211412845,
1.7791657320703511,
1.7366017573551213,
1.7515311999692533,
1.9126029849311892,
1.9404586817758533,
2.3972166377169897,
2.335279232323946,
2.287196195643049,
2.3233425451945937,
2.290758711558851,
2.1469598353913883,
2.06858151931004,
1.724673645627519,
1.7276797443832042,
1.6868743168799927,
1.750113598928641,
1.8689370302257808,
1.9201013561786646,
1.7571755878823676,
1.8064593932079833,
1.6182380848860212,
1.54734708262877,
1.6971709876766958,
1.9281851135734605,
1.9181459706422324,
1.8715616339625976,
1.98139109918343,
1.9725098609045926,
1.567179006207413,
1.646823927386757,
1.8889131565236124,
2.037922205205473,
1.9578401392089175,
1.9977416253788642,
1.7088731299455027,
1.7043167574088514,
1.6317539241945194,
1.6569662277885508,
1.851686828939778,
1.7819792021934393,
1.8201314936823685,
2.1827988871120168,
2.136585199982795,
2.147179272510316,
1.8247394247547277,
1.8425283782814323,
1.8381670098084166,
1.8380641584309292,
1.940393334356001,
1.980233601703622,
1.9484042613474806,
1.8870519883340404,
1.9146685903757994,
1.7213033230114907,
1.6944730203707439,
1.7982406378777354,
1.8487630347924604,
2.379768802448301,
2.428324916182792,
2.672933564082578,
2.123096738779665,
2.1350977246700134,
2.3142716827403076,
2.309932665923563,
3.1385535635378843,
3.083223329299671,
1.6829626734509104,
1.7228868849635557,
1.6641980021385072,
1.886830724464882,
1.825545612943488,
1.8665474219090596,
1.8597952962384958,
1.811460740982967,
1.8099207115470712,
1.900979695941989,
1.9101529661907906,
1.9127065857043086,
1.8950390106859525,
1.8347736338884606,
1.824808893457212,
1.6623844179585954
],
'right_track_width': [
2.428641507056224,
2.0105553299076435,
1.980887914799912,
2.0612493746189586,
2.1668725142616854,
2.24143863679738,
2.036840570765864,
1.9716438809362185,
1.8195089011725323,
1.8352916427922,
1.8923968030022722,
1.886440068840443,
2.3102152090947885,
2.377148043214572,
2.1195820358356654,
1.9730309965644843,
2.4089327989506693,
2.6729400346761,
2.523603599187183,
2.5336925487861395,
2.39535200277534,
1.8906025523063656,
2.025516323544674,
1.6737448193926165,
1.6967927494316117,
1.7463030425974775,
1.7629549826117856,
1.8139369494145436,
1.802929396890089,
2.062646347549259,
1.8974208616313495,
2.272391655910199,
2.2578705197745554,
2.15335350986316,
2.1849346610117046,
2.227948147677353,
1.7932701802540365,
1.8233039355717904,
1.7177601485537226,
1.7358630481441495,
1.7553651588580732,
1.9171883745962528,
1.9542792567113279,
1.8033481127766173,
1.6687644862524955,
1.6660500241920386,
1.7003207623548233,
1.6722522442024021,
1.775640719627485,
1.8920404678640763,
1.9205467565945382,
2.033039602219879,
1.6511912622950544,
1.576630108612005,
1.9867818967493165,
1.999797656507011,
1.9256705750679945,
1.9371707944360232,
1.6175065570713003,
1.7471037188174716,
1.6496723789877485,
1.621903419224135,
1.721276980590149,
1.6926078624250949,
1.7712168209728387,
1.92330899860879,
1.8940042870323333,
2.1412778173918747,
1.763250852589554,
1.8498097477996098,
1.8197317224419325,
1.8177292502348594,
1.8399480328552071,
1.8660872178746222,
2.009595035194049,
1.9953549787687606,
1.871497811532516,
1.7055953513900117,
1.7552543577592004,
1.6635891025052287,
1.6608517160394491,
2.0484624694037303,
1.9095614548543642,
2.4674155274073057,
2.431538747982309,
2.3298137736181173,
2.222550008559821,
2.258103192525677,
2.4299278526665145,
2.7123592645504377,
2.2999555931468816,
2.05772638294734,
1.7239265947213824,
1.6125508137623996,
1.707374482302558,
1.8464429036659658,
1.8045564685662585,
2.306272052620684,
1.7671227861606928,
1.7210037483994092,
1.825977288847786,
2.154306066083214,
1.9875586020383518,
1.6333918854553289,
1.3216154477477653,
1.2671787191702146,
1.4687191537861493
],
'closed_track': True,
'vehicle_pose': {
'header': {
'seq': 1017,
'stamp': {
'sec': 1639142725,
'nsec': 543047904
},
'frame_id': 'map'
},
'x': 11.389002983975667,
'y': -0.03622342456462392,
'v_x': 13.087446562548964,
'psi': 6.374352523887754,
'P': [
0.1577748404510498,
0.000042699999999999994,
0.007846000000000002,
0,
0.000042699999999999994,
0.19496114045104979,
0.0007139999999999998,
0,
0.007846000000000002,
0.0007139999999999998,
0.8197610112762451,
0,
0,
0,
0,
0.001012487025195352
]
}
}
vehicle_pose_msg = {
'header': {
'seq': 1216,
'stamp': {
'sec': 1639142730,
'nsec': 521801471
},
'frame_id': 'map'
},
'x': 7.873790533745808,
'y': 14.827524657458733,
'v_x': 7.415447801087341,
'psi': 10.429833030957056,
'P': [
0.16998576327819825,
0.00036320000000000005,
-0.004433900000000001,
0,
0.00036320000000000005,
0.21848716327819825,
-0.007146800000000002,
0,
-0.004433900000000001,
-0.007146800000000002,
0.9989876819549561,
0,
0,
0,
0,
0.0009624649627314633
]
}
[docs]def find_nearest_centerpoint(centerpoints, vehicle_position):
distance_to_vehicle = np.sqrt(np.sum(np.power(centerpoints - vehicle_position, 2), axis=1))
nearest_index = min(np.argmin(distance_to_vehicle) + 1, centerpoints.shape[0] - 1)
return np.argmax(distance_to_vehicle[nearest_index:] > 2) + nearest_index
[docs]def limit_global_centerpoints_by_cutting(centerpoints, vehicle_position, max_length: bool = True):
nearest_centerpoint_index = find_nearest_centerpoint(centerpoints=centerpoints, vehicle_position=vehicle_position)
if max_length is True:
cut_centerpoints = centerpoints[nearest_centerpoint_index:]
cum_distance = np.cumsum(np.sqrt(np.sum(np.power(np.diff(cut_centerpoints, axis=0,
prepend=cut_centerpoints[:1]), 2), axis=1)))
last_index = np.argmax(cum_distance > 15)
if last_index == 0:
last_index = None
else:
last_index = None
return (nearest_centerpoint_index), last_index
[docs]def limit_global_centerpoints_by_rolling(centerpoints, vehicle_position):
nearest_centerpoint_index = find_nearest_centerpoint(centerpoints=centerpoints, vehicle_position=vehicle_position)
rolled_centerpoints = np.roll(centerpoints, -(nearest_centerpoint_index), axis=0)
cum_distance = np.cumsum(np.sqrt(np.sum(np.power(np.diff(rolled_centerpoints, axis=0,
prepend=rolled_centerpoints[:1]), 2), axis=1)))
return -(nearest_centerpoint_index), np.argmax(cum_distance > 15)
finish_planning = True
if __name__ == '__main__':
with open('/workspace/as_ros/src/motion_planning/config/local_motion_planning.yaml', 'r') as config_file:
config = yaml.safe_load(config_file)
yappi.start()
local_motion_planner_config = local_motion_planner.LocalMotionPlannerConfig(
max_length_between_layer=config['interpolation']['max_length_between_layers'],
support_points_between_nodes_n=config['interpolation']['support_points_between_nodes_n'],
support_points_for_trajectory_n=config['interpolation']['support_points_for_trajectory_n'],
support_points_for_initial_interpolation_n=config[
'interpolation']['support_points_for_initial_interpolation_n'],
smooth_final_path=config['interpolation']['smooth_final_path'],
nodes_per_layer=config['interpolation']['nodes_per_layer'],
vehicle_width=config['track_width']['vehicle_width'],
buffer_to_trackbounds=config['track_width']['buffer_to_trackbounds'],
weight_length=config['optimization']['weights']['length'],
weight_max_curvature=config['optimization']['weights']['max_curvature'],
weight_average_curvature=config['optimization']['weights']['average_curvature'],
plot_path=True, plot_velocity_profile=True,
)
centerpoints = np.c_[centerpoints_msg['x'], centerpoints_msg['y']]
track_width = np.c_[centerpoints_msg['left_track_width'], centerpoints_msg['right_track_width']]
if centerpoints_msg['closed_track'] is True or finish_planning is True:
vehicle_position = np.array([vehicle_pose_msg['x'] + np.cos(vehicle_pose_msg['psi']) * 0.775,
vehicle_pose_msg['y'] + np.sin(vehicle_pose_msg['psi']) * 0.775])
vehicle_v = vehicle_pose_msg['v_x']
vehicle_psi = vehicle_pose_msg['psi']
if centerpoints_msg['closed_track'] is True:
roll, end = limit_global_centerpoints_by_rolling(centerpoints=centerpoints,
vehicle_position=vehicle_position)
centerpoints = np.roll(centerpoints, roll, axis=0)[:end]
track_width = np.roll(track_width, roll, axis=0)[:end]
else:
start, end = limit_global_centerpoints_by_cutting(centerpoints=centerpoints,
vehicle_position=vehicle_position)
centerpoints = centerpoints[start:][:end]
track_width = track_width[start:][:end]
else:
vehicle_position = np.array(
[centerpoints_msg['vehicle_pose']['x'] + np.cos(centerpoints_msg['vehicle_pose']['psi']) * 0.775,
centerpoints_msg['vehicle_pose']['y'] + np.sin(centerpoints_msg['vehicle_pose']['psi']) * 0.775])
vehicle_v = centerpoints_msg['vehicle_pose']['v_x']
vehicle_psi = centerpoints_msg['vehicle_pose']['psi']
start, end = limit_global_centerpoints_by_cutting(centerpoints=centerpoints, vehicle_position=vehicle_position,
max_length=False)
centerpoints = centerpoints[start:][:end]
track_width = track_width[start:][:end]
local_motion_planner_obj = local_motion_planner.LocalMotionPlanner(
local_motion_planner_config=local_motion_planner_config,
centerpoints=centerpoints,
track_width=track_width,
vehicle_psi=vehicle_pose_msg['psi'], vehicle_v=vehicle_pose_msg['v_x'],
vehicle_position=vehicle_position,
velocity_profile_config=velocity_profile.VelocityProfileConfig(
g=config['velocity_profile']['g'],
a_x_factor=config['velocity_profile']['a_x_factor'],
a_y_factor=config['velocity_profile']['a_y_factor'],
a_x_factor_completed=config['velocity_profile']['a_x_factor_completed'],
v_max=config['velocity_profile']['v_max'],
v_end=config['velocity_profile']['v_end'],
stretch_time_for_mpc=config['velocity_profile']['stretch_time_for_mpc'],
mpc_min_time_horizont=config['velocity_profile']['mpc_min_time_horizont']
)
)
local_motion_planner_obj.plan()
yappi.stop()
path = '/workspace/as_ros/rosbags/profiling/callgrind.out_debug_local_motion_planning'
yappi.get_func_stats().save(path, type='callgrind')
os.system(f'gprof2dot -w -s -f callgrind "{path}" | dot -Tsvg -o "{path}.svg"')
local_motion_planner_obj.plot()