An issue of visualization

CommonRoad-IO
1

Dear all,

I tried to visualize generated path roll-out by following code:

path_point_list = []
for n_path in range(len(paths)):
    plt.figure(figsize=(25, 10))
    draw_object(scenario, draw_params={'time_begin': 0})
    draw_object(planning_problem_set)
    collision_free = collision_check_array[n_path]
    is_best_path = (best_index == n_path)
    for n_point in range(len(paths[n_path][0])):
        point_state = State()
        point_state.position = [paths[n_path][0][n_point], paths[n_path][1][n_point]]
        point_state.time_step = n_point
        path_point_list.append(point_state)
    path_traj = Trajectory(1, path_point_list)
    if collision_free and is_best_path:
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'g'})
    elif collision_free and (not is_best_path):
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'b'})
    elif (not collision_free) and (not is_best_path):
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'y'})
    else:
        print('Error!!!')
    plt.show()

I have generated 7 paths, but only 4 are shown in commonRoad Visualization, some paths are covered by the road. And I could not change the color of path.

Do you have some suggestions for it?
Thank you!

image
image2500×1000 70.5 KB

image
image2500×1000 121 KB

BR
Kailin

2

Usually, this should not be a problem. From your example, it’s not straight forwards to see where you plot those 7 paths. Is inside the outer for-loop? If you provide a small running example it would be easier to help.

By the way, the ‘position’ of a state should usually be of type np.array() instead of a list, maybe that is causing some issues at some point.

3

Since only the colliding trajectories have been plotted, perhaps your collision_check_array function is working the other way around?

4

Thanks for reply,

I updated my code and the issue remains:

plt.figure(figsize=(25, 10))
path_point_list = []
draw_object(scenario, draw_params={'time_begin': 0})
draw_object(planning_problem_set)
for n_path in range(len(paths)):
    collision_free = collision_check_array[n_path]
    is_best_path = (best_index == n_path)
    for n_point in range(len(paths[n_path][0])):
        point_state = State()
        point_state.position = np.array([paths[n_path][0][n_point], paths[n_path][1][n_point]])
        point_state.time_step = n_point
        path_point_list.append(point_state)
    path_traj = Trajectory(1, path_point_list)
    if collision_free and is_best_path:
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'g'})
    elif collision_free and (not is_best_path):
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'b'})
    elif not collision_free :
        draw_object(path_traj, draw_params={'time_begin': 1, 'facecolor': 'r'})
    else:
        print('Error!!!')
plt.show()

plt.figure(figsize=(25, 10))
waypoints_np = np.array(waypoints)
waypoints_x = waypoints_np[:, 0]
waypoints_y = waypoints_np[:, 1]
plt.scatter(waypoints_x, waypoints_y, c='b', marker='.')
c_g = goal_state_planning.position.center
plt.scatter(c_g[0], c_g[1], c='r', marker='o', linewidths='10.0')
parkedcar_box_pts_np = np.array(parkedcar_box_pts)
plt.scatter(parkedcar_box_pts_np[:, 0], parkedcar_box_pts_np[:, 1], c='r', marker='s', linewidths='30.0')

for n_path in range(len(paths)):
    collision_free = collision_check_array[n_path]
    is_best_path = (best_index == n_path)
    if collision_free and is_best_path:
        plt.plot(paths[n_path][0], paths[n_path][1], c='g')
    elif collision_free and (not is_best_path):
        plt.plot(paths[n_path][0], paths[n_path][1], c='b')
    elif not collision_free:
        plt.plot(paths[n_path][0], paths[n_path][1], c='r')
    else:
        print('Error!!!')

plt.xlim([0, 200])
plt.ylim([-5, 35])
plt.show()

image
image2500×1000 119 KB

image
image2500×1000 77.9 KB

I am glad to support it, how can I upload a simple example?

BR
Kailin

5

I was talking about a complete code example that I could run to identify your problem, you can just attach it to your comment. However, did you try to run your example without plotting the scenario to make sure the trajectories are plotted?

6

import os
from commonroad.common.file_reader import CommonRoadFileReader
import matplotlib.pyplot as plt
from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.scenario.trajectory import Trajectory, State
import numpy as np

file_path = os.path.join(os.getcwd(), ‘ZAM_Over-1_1.xml’)
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()

paths = [[[30.59469721032756, 31.19463916511536, 31.794657398047118, 32.394764158056546, 32.99495049133625, 33.59518675159293, 34.19542493951406, 34.79560234229173, 35.39564600982714, 35.995477666513494, 36.595018716713035, 37.194195058445956, 37.79294147343755, 38.39120541240141, 38.9889500420456, 39.58615646449714, 40.182825060380814, 40.778975943439335, 41.37464854720132, 41.96990039273978, 42.56480511105486, 43.15944981417899, 43.753931925913115, 44.348355596364684, 44.94282783437758, 45.53745449870101, 46.132336292471216, 46.72756490634143, 47.32321945338438, 47.91936333362312, 48.51604165757357, 49.11327934630701, 49.71108001004333, 50.30942568794995, 50.90827750846392, 51.50757730196061, 52.10725016592923, 52.707207947057015, 53.30735356494692, 53.907586058867054, 54.50780619228843, 55.10792240035986, 55.707856813205595, 56.30755103320732, 56.9069712872611, 57.50611251514738, 58.105000892098666, 58.703694216602, 59.30227952243403], [-1.1294829064743912, -1.1102350089185622, -1.0935772016115477, -1.0805586565157324, -1.072064477191671, -1.068823217189879, -1.0714143328209604, -1.0802756410591388, -1.095710842639172, -1.117897153132098, -1.1468930648108524, -1.1826462423667534, -1.225001538102176, -1.2737090984719046, -1.3284325245292308, -1.3887570441941157, -1.4541976541275705, -1.5242071928777632, -1.5981843141382397, -1.6754813385577463, -1.755411973620308, -1.8372589027184112, -1.9202812557594593, -2.0037219836519116, -2.086815167111121, -2.1687932958538565, -2.248894557031602, -2.3263701714844087, -2.400491813066499, -2.470559140079435, -2.5359074591125554, -2.595915530874578, -2.650013515606401, -2.6976910432320205, -2.7385053814751004, -2.7720896647488225, -2.7981611387324405, -2.8165293711439565, -2.8271043791457817, -2.829904628716336, -2.82506487153165, -2.8128438003895293, -2.7936315244575454, -2.767956889539332, -2.7364946943561472, -2.7000728789724673, -2.659679782497908, -2.616471579592765, -2.5717800034439633], [0.03495, 0.03375837769290259, 0.0303854863279191, 0.025124592430585423, 0.018256033447867786, 0.010047217748162796, 0.0007526246212974097, -0.009386195721471091, -0.020140622147455045, -0.031294962602536426, -0.04264645411116683, -0.05400526277636756, -0.06519448377972953, -0.0760501413814133, -0.0864211889201491, -0.09616950881323674, -0.10516991255654584, -0.11331014072451531, -0.12049086297015424, -0.1266256780250406, -0.13164111369932308, -0.13547662688171888, -0.13808460353951543, -0.13943035871857012, -0.13949213654330916, -0.1382611102167292, -0.1357413820203959, -0.13194998331444505, -0.12691687453758163, -0.12068494520708092, -0.11331001391878684, -0.1048608283471146, -0.09541906524504687, -0.08507933044413732, -0.07394915885450962, -0.06214901446485658, -0.04981229034244009, -0.0370853086330916, -0.024127320561213396, -0.011110506429777879, 0.0017800243796750256, 0.014346233407035412, 0.02637715311362184, 0.037648886882185116, 0.04792460901690606, 0.05695456474339374, 0.06447607020869014, 0.07021351248126725, 0.07387834955102754]], [[30.591751274176097, 31.188732619384652, 31.78576718379528, 32.382868396307174, 32.98003923947512, 33.57727258102889, 34.17455229789813, 34.77185495824893, 35.36915185689949, 35.96641122774144, 36.56360048354891, 37.160688358861265, 37.75764685549013, 38.35445291260456, 38.951089744247334, 39.54754780647667, 40.14382537406, 40.73992872272689, 41.33587192738997, 41.931676299455326, 42.5273694973755, 43.12298435397115, 43.71855747179354, 44.314127643958656, 44.909734162490345, 45.50541507929788, 46.101205486502565, 46.6971358829274, 47.29323069216679, 47.88950699473919, 48.48597353236256, 49.08263003634011, 49.679466924360426, 50.276465400660626, 50.87359798344684, 51.47082947069581, 52.06811834098151, 52.66541856980793, 53.26268182412998, 53.85985997837687, 54.45690787442873, 55.05378622571507, 55.65046454196302, 56.24692392615386, 56.84315956893537, 57.43918273800776, 58.03502203071215, 58.63072362699087, 59.22635024680107], [-1.1294631190378202, -1.1097250376923828, -1.091685719259726, -1.076030713146168, -1.063336338163, -1.054074815085223, -1.048619371943989, -1.047249345166186, -1.0501552960244458, -1.0574441563802244, -1.0691444115270154, -1.0852113219186226, -1.1055321802979838, -1.129931596610768, -1.15817680029974, -1.189982948186668, -1.2250184260948167, -1.2629101334918837, -1.3032487425198058, -1.3455939255612213, -1.3894795486884992, -1.434418831663658, -1.4799094783333797, -1.5254387840469863, -1.5704887289091558, -1.6145410671025997, -1.6570824230713601, -1.6976094049927546, -1.735633744694883, -1.7706874710664096, -1.802328121183478, -1.8301439900264738, -1.853759416006206, -1.872840095833439, -1.887098418844409, -1.8962988080494623, -1.9002630532121758, -1.8988756204816681, -1.8920889237389307, -1.8799285450609684, -1.8624983956450736, -1.8399858141415195, -1.8126666064399481, -1.7809100391875237, -1.7451838081191124, -1.7060590108396019, -1.6642151609223106, -1.6204452846598136, -1.5756611417427067], [0.03495, 0.03416339864290258, 0.03193881325197821, 0.02847253617197589, 0.023952145513240847, 0.018556505151714538, 0.012455764728934643, 0.005811359652035081, -0.0012239889062540255, -0.008506274007606313, -0.015900202948099164, -0.02327919725821377, -0.03052539270283513, -0.03752963928125189, -0.04419150122715664, -0.050419257008645586, -0.05612989932821879, -0.06124913512278011, -0.06571138556363709, -0.06945978605650105, -0.07244618624148724, -0.07463114999311449, -0.07598395542030553, -0.07648259486638681, -0.07611377490908852, -0.07487291636054477, -0.07276415426729302, -0.06980033791027532, -0.06600303080483666, -0.061402510700726144, -0.05603776958209653, -0.04995651366750479, -0.04321516340991103, -0.035878853496679665, -0.02802143284957862, -0.019725464624779344, -0.011082226212857739, -0.002191709238792673, 0.0068373804380324685, 0.015887622723830895, 0.024832883290411965, 0.03353831357518141, 0.041860350781141885, 0.0496467178768902, 0.0567364235966212, 0.06295976244012566, 0.06813831467278939, 0.07208494632559656, 0.07460380919512596]], [[30.58978960234066, 31.18479395630044, 31.779825286667837, 32.37489251789597, 32.97000118578705, 33.56515359833408, 34.16034917966036, 34.755584939140604, 35.35085601550025, 35.94615625298233, 36.5414787735518, 37.13681651557914, 37.732162715510675, 38.327511314690334, 38.9228572787447, 39.51819682177701, 40.11352753203289, 40.70884839969924, 41.30415975107536, 41.89946309651223, 42.494760902251784, 43.09005629861465, 43.68535273888284, 44.280653624706126, 44.87596191492817, 45.47127973538208, 46.06660800744588, 46.66194611297476, 47.25729161263973, 47.852640033697405, 48.447984741792276, 49.043316909548395, 49.63862559243921, 50.23389791973162, 50.8291194051828, 51.424274378627025, 52.01934653562715, 52.6143195979864, 53.20917807312593, 53.80390809513849, 54.39849832473561, 54.99294087931965, 55.58723225803651, 56.1813742198999, 56.775374565916714, 57.369247768576265, 57.96301538407366, 58.55670617419503, 59.15035585587009], [-1.1294095481089061, -1.1091499353795735, -1.0897024166079765, -1.0713908836918753, -1.054484528119535, -1.0392004934803538, -1.0257065219360935, -1.0141235983200936, -1.0045285948989102, -0.9969569190138232, -0.9914051649542083, -0.9878337706000961, -0.9861696786751653, -0.9863090019151131, -0.9881196910988214, -0.9914442047119305, -0.9961021790011306, -1.001893097308827, -1.0085989578206442, -1.0159869391769192, -1.0238120637565649, -1.0318198588007474, -1.0397490158705884, -1.0473340493978027, -1.054307955265745, -1.0604048704335236, -1.0653627345779637, -1.068925954575727, -1.070848072387415, -1.0708944365518775, -1.068844877074635, -1.0644963830288152, -1.057665781715666, -1.0481924177944748, -1.0359408304314321, -1.0208034262773333, -1.0027031460072684, -0.981596122279839, -0.9574743273301298, -0.930368209020657, -0.9003493150449788, -0.8675329060992922, -0.832080560176141, -0.7942027716334581, -0.7541615502630508, -0.7122730271013507, -0.6689100750270887, -0.624504953064398, -0.5795519834921565], [0.03495, 0.03457093879087587, 0.0335018024426904, 0.03184130694391518, 0.029683695042610016, 0.027118736246422945, 0.024231726822590248, 0.021103489797936425, 0.01781037495887422, 0.014424258851404595, 0.01101254478111676, 0.007638162813188149, 0.004359569772384438, 0.0012307492430594993, -0.0016987884308445056, -0.004384006145797231, -0.006784340038680073, -0.00886369948678619, -0.010590467107820571, -0.01193749875989989, -0.012882123541552597, -0.01340614379171897, -0.01349583508975101, -0.01314194625541247, -0.012339699348878928, -0.011088789670737793, -0.009393385761987928, -0.007262129404040314, -0.004708135618717572, -0.0017489926682541304, 0.0015932379447039999, 0.005292021477098806, 0.00931634994546051, 0.013630742125907834, 0.01819524355414761, 0.022965426525474474, 0.027892390094771978, 0.032922760076511424, 0.0379986890447527, 0.04305785633314387, 0.0480334680349205, 0.052854257002907544, 0.057444482849517496, 0.06172393194675126, 0.06560791742619762, 0.06900727917903435, 0.07182838385602694, 0.07397312486752869, 0.07533892238348236]], [[30.58881808355787, 31.182834909851668, 31.77684920070963, 32.37085965098643, 32.964864926097086, 33.558863659507836, 34.15285445030282, 34.74683586094628, 35.34080641536012, 35.93476459743571, 36.52870885009682, 37.122637575027746, 37.716549133176834, 38.31044184614071, 38.90431399852875, 39.49816384140057, 40.09198959686146, 40.68578946389222, 41.27956162547968, 41.8733042571041, 42.467015536627606, 43.0606936556153, 43.65433683210727, 44.24794332484501, 44.841511448940295, 45.43503959295829, 46.02852623736896, 46.6219699743027, 47.2153695285268, 47.808723779538724, 48.40203178465171, 48.995292802925, 49.588506319768946, 50.181672072030345, 50.774790073338856, 51.36786063946919, 51.96088441344686, 52.5538623900975, 53.14679593971096, 53.73968683046151, 54.332537249194786, 54.9253498201606, 55.51812762123754, 56.11087419716219, 56.7035935692406, 57.2962902409841, 57.88896919907465, 58.48163590902712, 59.074296304877016], [-1.1293216713282812, -1.10850744620103, -1.0876210194154354, -1.0666256980285749, -1.045484529810746, -1.0241604317066817, -1.0026163183026349, -0.980815230297126, -0.9587204629728967, -0.9362956946672797, -0.9135051152378122, -0.8903135545194742, -0.8666866107694514, -0.8425907790947991, -0.8179935798578583, -0.7928636870537419, -0.7671710566536977, -0.7408870549077009, -0.7139845865992188, -0.6864382232447915, -0.6582243312308763, -0.629321199880351, -0.5997091694411874, -0.569370758990106, -0.5382907942445467, -0.5064565352770378, -0.4738578041270659, -0.4404871123068308, -0.4063397881988494, -0.37141410434524896, -0.33571140463077775, -0.29923623136405064, -0.2619964522643478, -0.22400338736436912, -0.1852719358427079, -0.1458207028034169, -0.10567212602384207, -0.064852602695885, -0.023392616189913895, 0.018673137125336803, 0.06130562096949965, 0.10446133210265424, 0.14809217309783568, 0.19214532488581315, 0.23656311902128208, 0.2812829096176347, 0.32623694489637056, 0.37135223829724207, 0.41655043909656786], [0.03495, 0.034980061113894845, 0.03507085999933514, 0.035223158104124165, 0.0354375010522914, 0.035714218644092514, 0.036053424856009396, 0.036455017840750094, 0.03691867992724888, 0.03744387762066621, 0.03802986160238875, 0.03867566673002933, 0.039380112037427015, 0.04014180073464704, 0.04095912020798085, 0.04183024201994607, 0.04275312190928653, 0.043725499790972275, 0.04474489975619951, 0.045808630072390666, 0.04691378318319434, 0.04805723570848536, 0.04923564844436473, 0.05044546636315964, 0.0516829186134235, 0.052944018519935895, 0.05422456358370263, 0.05552013548195568, 0.05682610006815325, 0.05813760737197968, 0.059449591599345555, 0.06075677113238766, 0.06205364852946895, 0.06333451052517859, 0.06459342803033194, 0.06582425613197057, 0.0670206340933622, 0.06817598535400081, 0.06928351752960651, 0.07033622241212564, 0.07132687596973074, 0.07224803834682056, 0.07309205386401998, 0.07385105101818018, 0.07451694248237845, 0.07508142510591828, 0.07553597991432942, 0.07587187210936773, 0.07608015106901536]], [[30.58884015628568, 31.182862312194203, 31.776849063091756, 32.370783104724175, 32.96464676336311, 33.55842183820634, 34.15208968773694, 34.745631499055705, 35.339028686182914, 35.932263369978884, 36.525318898658846, 37.11818037387576, 37.71083515301368, 38.303273303670416, 38.89548799130744, 39.48747578570281, 40.07923687615441, 40.67077518934189, 41.26209840736424, 41.853217886722895, 42.44414848191938, 43.03490827988123, 43.62551825362393, 44.21600184540286, 44.80638449111183, 45.39669309884775, 45.98695549538854, 46.57719985482704, 47.16745412376937, 47.75774545734468, 48.348099679783516, 48.938540782504234, 49.52909047149907, 50.119767774329986, 50.7105887152274, 51.30156606462681, 51.89270916697706, 52.4840238478057, 53.075512397828405, 53.66717362834046, 54.25900298822741, 54.85099272868018, 55.4431320970994, 56.035407536727085, 56.62780286325517, 57.220299384033694, 57.81287591954227, 58.405508680502486, 58.99817094740236], [-1.1291990455551941, -1.1077954481682746, -1.0854353953804639, -1.061721771551178, -1.0363117764201375, -1.008914536299002, -0.9792887080983239, -0.9472400757438473, -0.9126191384520936, -0.8753186906181276, -0.8352713935572503, -0.7924473399112224, -0.7468516120834605, -0.6985218365385785, -0.6475257361449681, -0.5939586829299588, -0.5379412536469568, -0.4796167904277033, -0.4191489685256498, -0.356719372771435, -0.29252508388687776, -0.22677627527117733, -0.15969382031470392, -0.09150690974384357, -0.022450677984763634, 0.047236162918670166, 0.11731368366489558, 0.18754309442931805, 0.25768910022071323, 0.3275222571366363, 0.3968213317054031, 0.4653756652987513, 0.5329875452938588, 0.5994745842727457, 0.6646721080923844, 0.7284355531644862, 0.7906428727764656, 0.8511969517917755, 0.9100280286148015, 0.96709612291618, 1.0223934673069595, 1.0759469409351987, 1.1278205028570971, 1.1781176229942627, 1.2269837085008417, 1.2746085233813704, 1.3212285991516004, 1.367129634123158, 1.4126488783908404], [0.03495, 0.03538981085122637, 0.036642323856927006, 0.0386101957826669, 0.04120013151072122, 0.04432288404007534, 0.04789325448642481, 0.051830092082175414, 0.05605629417644312, 0.06049880623505407, 0.06508862184054463, 0.06976078269216138, 0.07445437860586106, 0.07911254751431061, 0.0836824754668872, 0.08811539662967818, 0.09236659328548111, 0.09639539583380372, 0.10016518279086398, 0.10364338078958998, 0.10680146457962009, 0.1096149570273029, 0.11206342911569706, 0.11413049994457153, 0.11580383673040553, 0.11707515480638828, 0.11794021762241938, 0.11839883674510843, 0.11845487185777556, 0.1181162307604508, 0.11739486936987445, 0.11630679171949701, 0.11487204995947925, 0.113114744356692, 0.11106302329471654, 0.108749083273844, 0.10620916891107615, 0.10348357294012431, 0.10061663621141068, 0.09765674769206742, 0.09465634446593627, 0.09167191173357062, 0.0887639828122325, 0.08599713913589521, 0.08344001025524136, 0.08116527383766467, 0.07924965566726858, 0.07777392964486682, 0.07682291778798309]], [[30.58985679787322, 31.18487807285801, 31.779827624946826, 32.37466633382704, 32.969350668497135, 33.56383238546492, 34.158059143677605, 34.75197579420869, 35.34552613122604, 35.93865491793975, 36.531310027119176, 37.12344456039288, 37.71501883389098, 38.306002139816215, 38.896374214179616, 39.48612636014298, 40.07526219410733, 40.66379799782002, 41.2517626743055, 41.839197318336936, 42.42615442346375, 43.012696757315524, 43.598895945057414, 44.18483080752642, 44.77058550579052, 45.35624754769883, 45.94190571448219, 46.52764796665939, 47.11355938842788, 47.69972022837797, 48.28620409175554, 48.87307633558316, 49.46039271269444, 50.04819830409071, 50.636526770939355, 51.22539994794735, 51.81482778871524, 52.404808660972535, 52.99532997529851, 53.58636911504103, 54.17789461768171, 54.769867538891205, 55.36224291002391, 55.954971177868494, 56.547999492155554, 57.14127268167238, 57.73473373388476, 58.32832356574664, 58.92197984492071], [-1.1290413138710684, -1.10701199236203, -1.0831396732622418, -1.05666607040363, -1.0269421441148934, -0.9934232530055419, -0.9556642801210682, -0.9133147231790095, -0.8661137394035221, -0.8138851388564285, -0.7565323248692986, -0.6940331852520777, -0.6264349426770837, -0.5538489765183523, -0.47644563115760613, -0.3944490271991766, -0.3081318921461893, -0.21781042596140554, -0.12383921472885262, -0.02660620256277879, 0.07347227177065407, 0.17595637107903672, 0.28038759047935113, 0.3862935282316826, 0.4931926057782593, 0.600598749012248, 0.708026044960941, 0.8149933893618345, 0.921029140976904, 1.025675797919748, 1.1284947098017737, 1.2290708372252115, 1.3270175671954934, 1.421981589566125, 1.513647835871649, 1.6017444780784094, 1.6860479811315954, 1.7663881999431161, 1.842653508874911, 1.914795950019141, 1.9828363857989135, 2.046869641672511, 2.107069625981231, 2.163694416068675, 2.2170913023950733, 2.267701784965614, 2.3160665182622324, 2.362830201039716, 2.408746404561719], [0.03495, 0.03579921739060979, 0.038212471356768425, 0.04199439596371678, 0.04695793377502496, 0.052924335852592354, 0.05972316175664756, 0.06719227954574847, 0.0751778657767822, 0.08353440550496513, 0.09212469228384287, 0.10081982816529034, 0.10949922369951165, 0.11805059793504016, 0.12636997841873857, 0.13436170119579868, 0.14193841080974176, 0.1490210603024181, 0.15553891121400734, 0.1614295335830184, 0.1666388059462895, 0.17112091533898785, 0.17483835729461036, 0.17776193584498287, 0.1798707635202603, 0.18115226134892728, 0.1816021588577975, 0.1812244940720138, 0.18003161351504826, 0.1780441722087025, 0.17529113367310692, 0.17180976992672184, 0.16764566148633606, 0.16285269736706812, 0.1574930750823658, 0.15163730064400568, 0.14536418856209452, 0.13876086184506717, 0.1319227519996886, 0.12495359903105305, 0.11796545144258316, 0.1110786662360311, 0.1044219089114797, 0.0981321534673398, 0.09235468240035002, 0.08724308670558109, 0.0829592658764324, 0.0796734279046298, 0.07756408928023226]], [[30.591866481185633, 31.188879091965557, 31.785780121151518, 32.38250276494786, 32.978968063408644, 33.57508446962875, 34.17074944077225, 34.765852507204244, 35.36027933997195, 35.95391639800345, 36.54665579492107, 37.138400081423335, 37.72906669273032, 38.318591861429525, 38.90693384391967, 39.494075353205545, 40.08002513173073, 40.66481863498414, 41.24851782960251, 41.831210138536, 42.41300659057654, 42.99403925229282, 43.57445803737216, 44.15442700179175, 44.73412024340331, 45.313717531682705, 45.89339979779901, 46.47334461696615, 47.05372181435018, 47.63468932262558, 48.21638941353165, 48.79894541732281, 49.38245903262478, 49.96700831464982, 50.55264641172907, 51.13940109844119, 51.72727512805523, 52.316247397429535, 52.90627488387245, 53.497295275824655, 54.089230177706916, 54.68198872410715, 55.275471389920845, 55.86957373138369, 56.46418973838822, 57.059214421234806, 57.65454519511451, 58.250081563161466, 58.84572253382136], [-1.1288482067745296, -1.1061553041872274, -1.0807282422445872, -1.0514459063875856, -1.017351939185787, -0.9776475415033129, -0.9316842200944041, -0.8789564394486199, -0.8190941400967248, -0.7518550982653428, -0.6771171185697109, -0.5948700691554007, -0.50520778498238, -0.4083198781669881, -0.30448350347410735, -0.19405513173475508, -0.07746238414584095, 0.044804023571142104, 0.1721981857178575, 0.3041267287660694, 0.4399563322288511, 0.5790210425990262, 0.7206293879984675, 0.8640713137237117, 1.0086249699117638, 1.1535633914640997, 1.2981611166788904, 1.441700794429254, 1.5834798300298192, 1.7228171171617883, 1.8590598975490538, 1.9915907818245913, 2.1198349546758313, 2.243267575521728, 2.3614213733762845, 2.4738944219993484, 2.5803580697766533, 2.6805649888570757, 2.7743573006974493, 2.8616747309921777, 2.942562746465647, 3.01718062936805, 3.0858094525570228, 3.1488599281201077, 3.206880114383959, 3.2605629779861762, 3.310753816802527, 3.3584575523906555, 3.404845892695641], [0.03495, 0.03620732002147536, 0.03977761766867129, 0.04536781583221143, 0.05269739275801111, 0.06149838204727743, 0.07151537265650923, 0.08250550889749711, 0.09423849043732334, 0.10649657229836204, 0.11907456485827894, 0.1317798338500317, 0.14443230036186946, 0.15686444083733336, 0.16892128707525622, 0.18046042622976238, 0.19135200081026812, 0.20147870868148163, 0.21073580306340237, 0.219031092531322, 0.22628494101582378, 0.23243026780278253, 0.23741254753336513, 0.2411898102040297, 0.2437326411665267, 0.24502418112789814, 0.24506012615047743, 0.2438487276518901, 0.24141079240505361, 0.2377796825381762, 0.23300131553475903, 0.2271341642335938, 0.22024925682876537, 0.21243017686964957, 0.2037730632609138, 0.19438661026251688, 0.18439206748971101, 0.17392323991303835, 0.16312648785833292, 0.15216072700672265, 0.14119742839462376, 0.1304206184137475, 0.12002687881109406, 0.11022534668895775, 0.10123771450492433, 0.09329823007186785, 0.08665369655795915, 0.0815634724866576, 0.07829947173671467]]]
best_index = 6
collision_check_array = [ True, True, False, False, False, True, True]

plt.figure(figsize=(25, 10))
path_point_list = []
draw_object(scenario, draw_params={‘time_begin’: 0})
draw_object(planning_problem_set)
for n_path in range(len(paths)):
collision_free = collision_check_array[n_path]
is_best_path = (best_index == n_path)
for n_point in range(len(paths[n_path][0])):
point_state = State()
point_state.position = np.array([paths[n_path][0][n_point], paths[n_path][1][n_point]])
point_state.time_step = n_point
path_point_list.append(point_state)
path_traj = Trajectory(1, path_point_list)
if collision_free and is_best_path:
draw_object(path_traj, draw_params={‘time_begin’: 1, ‘facecolor’: ‘g’})
print(n_path, ‘is green path’)
elif collision_free and (not is_best_path):
draw_object(path_traj, draw_params={‘time_begin’: 1, ‘facecolor’: ‘b’})
print(n_path, ‘is blue path’)
elif not collision_free :
draw_object(path_traj, draw_params={‘time_begin’: 1, ‘facecolor’: ‘r’})
print(n_path, ‘is red path’)
else:
print(‘Error!!!’)
plt.show()

7

My code is attached:

Two issues:

  1. the color of each trajectory was not changed.
  2. some trajectories were not drawn, but for sure the draw_objective function was called.

Output on the screen:

0 is blue path
1 is blue path
2 is red path
3 is red path
4 is red path
5 is blue path
6 is green path

picture:

image
image2500×1000 119 KB

8

  1. to specify the color, you have to specify it like

    draw_params = {‘trajectory’:{‘facecolor’:‘g’}}
    or
    draw_params = {‘scenario’:{‘dynamic_obstacle’:{‘trajectory’:{‘facecolor’:‘g’}}}}

    as described here https://commonroad.in.tum.de/static/docs/commonroad-io/user/visualization.html#passing-custom-draw-params. Since facecolor exists in multiple objects, you have to specify ‘trajectory’ as well in the first option to specify it unambiguously.

  2. I plotted it without the scenario and there were no hidden trajectories:

    image
    image1092×508 10.9 KB

    However, it seems like you plot the same trajectories multiple times in different colors (just plot inside every outer for loop into a different figure and have a look yourself). I assume this is not caused by commonroad-io.

1 Like