search

tpbarron / vehicle_optimization

Vehicle simulator to test algorithms for traffic optimization.
1 stars 0 forks source link

Fix route update calculation. Does not complete route. #50

Open tpbarron opened 9 years ago

tpbarron commented 9 years ago

Use graphics: 0 Intersections: data/scenarios/basic_grid_single_vehicle/basic_grid_intersections.json Roads: data/scenarios/basic_grid_single_vehicle/basic_grid_roads.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Hazard: data/scenarios/basic_grid_single_vehicle/hazards/hazard1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane1.json Lane: data/scenarios/basic_grid_single_vehicle/lanes/lane2.json Vehicle 1: data/scenarios/basic_grid_single_vehicle/vehicles/vehicle1.json Loaded vehicle data : 0, 0, 120, 120 (0 pos=(0, 0)), road = 100, (1 pos=(120, 0)) (0 pos=(0, 0)), road = 100, (3 pos=(0, 120)) (1 pos=(120, 0)), road = 100, (0 pos=(0, 0)) (1 pos=(120, 0)), road = 100, (2 pos=(240, 0)) (1 pos=(120, 0)), road = 100, (4 pos=(120, 120)) (2 pos=(240, 0)), road = 100, (1 pos=(120, 0)) (2 pos=(240, 0)), road = 100, (5 pos=(240, 120)) (3 pos=(0, 120)), road = 100, (0 pos=(0, 0)) (3 pos=(0, 120)), road = 100, (4 pos=(120, 120)) (4 pos=(120, 120)), road = 100, (1 pos=(120, 0)) (4 pos=(120, 120)), road = 100, (3 pos=(0, 120)) (4 pos=(120, 120)), road = 100, (5 pos=(240, 120)) (5 pos=(240, 120)), road = 100, (2 pos=(240, 0)) (5 pos=(240, 120)), road = 100, (4 pos=(120, 120)) Starting scenario Starting vehicle Routing from: 0 -> 4 vehicle start intersect pos: (0, 0) Computed angle to intersection in rad: 0 Vertex = 1 Vertex = 4 Starting ModuleManager Starting vehicle sensor module Starting Autopilot Module Updating self Position: (1.1175999999999999, 0) Heading: 1.5707963267948966

Updating self Position: (2.2351999999999999, 0) Heading: 1.5707963267948966

Updating self Position: (3.3527999999999998, 0) Heading: 1.5707963267948966

Updating self Position: (4.4592239999999999, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (5.5768240000000002, 0) Heading: 1.5707963267948966

Updating self Position: (6.6944239999999997, 0) Heading: 1.5707963267948966

Updating self Position: (7.8120239999999992, 0) Heading: 1.5707963267948966

Updating self Position: (8.9184479999999997, 0) Heading: 1.5707963267948966

Updating self Position: (10.036047999999999, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (11.142472, 0) Heading: 1.5707963267948966

Updating self Position: (12.260071999999999, 0) Heading: 1.5707963267948966

Updating self Position: (13.377671999999999, 0) Heading: 1.5707963267948966

Updating self Position: (14.495271999999998, 0) Heading: 1.5707963267948966

Updating self Position: (15.601695999999999, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (16.708119999999997, 0) Heading: 1.5707963267948966

Updating self Position: (17.825719999999997, 0) Heading: 1.5707963267948966

Updating self Position: (18.943319999999996, 0) Heading: 1.5707963267948966

Updating self Position: (20.049743999999997, 0) Heading: 1.5707963267948966

Updating self Position: (21.167343999999996, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (22.284943999999996, 0) Heading: 1.5707963267948966

Updating self Position: (23.391367999999996, 0) Heading: 1.5707963267948966

Updating self Position: (24.508967999999996, 0) Heading: 1.5707963267948966

Updating self Position: (25.615391999999996, 0) Heading: 1.5707963267948966

Updating self Position: (26.732991999999996, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (27.839415999999996, 0) Heading: 1.5707963267948966

Updating self Position: (28.957015999999996, 0) Heading: 1.5707963267948966

Updating self Position: (30.063439999999996, 0) Heading: 1.5707963267948966

Updating self Position: (31.169863999999997, 0) Heading: 1.5707963267948966

Updating self Position: (32.287464, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (33.393887999999997, 0) Heading: 1.5707963267948966

Updating self Position: (34.511488, 0) Heading: 1.5707963267948966

Updating self Position: (35.629088000000003, 0) Heading: 1.5707963267948966

Updating self Position: (36.735512, 0) Heading: 1.5707963267948966

Updating self Position: (37.853112000000003, 0) Heading: 1.5707963267948966

Updating self Position: (38.970712000000006, 0) Heading: 1.5707963267948966

VehicleSensorModule broadcasting Updating self Position: (40.077136000000003, 0) Heading: 1.5707963267948966

Updating self Position: (41.18356, 0) Heading: 1.5707963267948966

Updating self Position: (42.289983999999997, 0) Heading: 1.5707963267948966

Updating self Position: (43.407584, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (44.525184000000003, 0) Heading: 1.5707963267948966

Updating self Position: (45.631608, 0) Heading: 1.5707963267948966

Updating self Position: (46.738031999999997, 0) Heading: 1.5707963267948966

Updating self Position: (47.844455999999994, 0) Heading: 1.5707963267948966

Updating self Position: (48.962055999999997, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (50.079656, 0) Heading: 1.5707963267948966

Updating self Position: (51.186079999999997, 0) Heading: 1.5707963267948966

Updating self Position: (52.292503999999994, 0) Heading: 1.5707963267948966

Updating self Position: (53.410103999999997, 0) Heading: 1.5707963267948966

Updating self Position: (54.527704, 0) Heading: 1.5707963267948966

Updating self VehicleSensorModule broadcasting Position: (55.645304000000003, 0) Heading: 1.5707963267948966

Updating self Position: (56.762904000000006, 0) Heading: 1.5707963267948966

Updating self Position: (57.869328000000003, 0) Heading: 1.5707963267948966

Updating self Position: (58.986928000000006, 0) Heading: 1.5707963267948966

Updating self Position: (60.104528000000009, 0) Heading: 1.5707963267948966

Updating self Position: (61.210952000000006, 0) Heading: 1.5707963267948966

Updating self Position: (62.328552000000009, 0) Heading: 1.5707963267948966

Updating self Position: (63.434976000000006, 0) Heading: 1.5707963267948966

Updating self Position: (64.54140000000001, 0) Heading: 1.5707963267948966

Updating self Position: (65.659000000000006, 0) Heading: 1.5707963267948966

Updating self Position: (66.76542400000001, 0) Heading: 1.5707963267948966

Updating self Position: (67.871848000000014, 0) Heading: 1.5707963267948966

Updating self Position: (68.98944800000001, 0) Heading: 1.5707963267948966

Updating self Position: (70.095872000000014, 0) Heading: 1.5707963267948966

Updating self Position: (71.21347200000001, 0) Heading: 1.5707963267948966

Updating self Position: (72.319896000000014, 0) Heading: 1.5707963267948966

Updating self Position: (73.43749600000001, 0) Heading: 1.5707963267948966

Updating self Position: (74.543920000000014, 0) Heading: 1.5707963267948966

Updating self Position: (75.650344000000018, 0) Heading: 1.5707963267948966

Updating self Position: (76.767944000000014, 0) Heading: 1.5707963267948966

Updating self Position: (77.874368000000018, 0) Heading: 1.5707963267948966

Updating self Position: (78.991968000000014, 0) Heading: 1.5707963267948966

Updating self Position: (80.10956800000001, 0) Heading: 1.5707963267948966

Updating self Position: (81.215992000000014, 0) Heading: 1.5707963267948966

Updating self Position: (82.33359200000001, 0) Heading: 1.5707963267948966

Updating self Position: (83.440016000000014, 0) Heading: 1.5707963267948966

Updating self Position: (84.55761600000001, 0) Heading: 1.5707963267948966

Updating self Position: (85.664040000000014, 0) Heading: 1.5707963267948966

Updating self Position: (86.770464000000018, 0) Heading: 1.5707963267948966

Updating self Position: (87.876888000000022, 0) Heading: 1.5707963267948966

Updating self Position: (88.994488000000018, 0) Heading: 1.5707963267948966

Updating self Position: (90.100912000000022, 0) Heading: 1.5707963267948966

Updating self Position: (91.218512000000018, 0) Heading: 1.5707963267948966

Updating self Position: (92.324936000000022, 0) Heading: 1.5707963267948966

Updating self Position: (93.431360000000026, 0) Heading: 1.5707963267948966

Updating self Position: (94.548960000000022, 0) Heading: 1.5707963267948966

Updating self Position: (95.655384000000026, 0) Heading: 1.5707963267948966

Updating self Position: (96.772984000000022, 0) Heading: 1.5707963267948966

Updating self Position: (97.879408000000026, 0) Heading: 1.5707963267948966

Updating self Position: (98.98583200000003, 0) Heading: 1.5707963267948966

Updating self Position: (100.09225600000003, 0) Heading: 1.5707963267948966

Updating self Position: (101.20985600000003, 0) Heading: 1.5707963267948966

Updating self Position: (102.31628000000003, 0) Heading: 1.5707963267948966

Updating self Position: (103.42270400000004, 0) Heading: 1.5707963267948966

Updating self Position: (104.54030400000003, 0) Heading: 1.5707963267948966

Updating self Position: (105.65790400000003, 0) Heading: 1.5707963267948966

Updating self Position: (106.77550400000003, 0) Heading: 1.5707963267948966

Updating self Position: (107.89310400000002, 0) Heading: 1.5707963267948966

Updating self Position: (108.99952800000003, 0) Heading: 1.5707963267948966

Updating self Position: (110.10595200000003, 0) Heading: 1.5707963267948966

Updating self Position: (111.21237600000003, 0) Heading: 1.5707963267948966

Updating self Position: (112.31880000000004, 0) Heading: 1.5707963267948966

Updating self Position: (113.43640000000003, 0) Heading: 1.5707963267948966

Updating self Position: (114.54282400000004, 0) Heading: 1.5707963267948966

Updating self Position: (115.64924800000004, 0) Heading: 1.5707963267948966

Updating self Position: (116.75567200000005, 0) Heading: 1.5707963267948966

Updating self Position: (117.86209600000005, 0) Heading: 1.5707963267948966

Updating self Position: (118.97969600000005, 0) Heading: 1.5707963267948966

Updating self Computed angle to intersection in rad: 1.5708 Position: (120, 0.086120000000046826) Heading: 1.6550029473451999

Updating self Position: (120, 1.2037200000000468) Heading: 3.1415926535897931

Updating self Position: (120, 2.3101440000000468) Heading: 3.1415926535897931

Updating self Position: (120, 3.4277440000000468) Heading: 3.1415926535897931

Updating self Position: (120, 4.5341680000000473) Heading: 3.1415926535897931

Updating self Position: (120, 5.6405920000000478) Heading: 3.1415926535897931

Updating self Position: (120, 6.7581920000000473) Heading: 3.1415926535897931

Updating self Position: (120, 7.8646160000000478) Heading: 3.1415926535897931

Updating self Position: (120, 8.9822160000000473) Heading: 3.1415926535897931

Updating self Position: (120, 10.088640000000048) Heading: 3.1415926535897931

Updating self Position: (120, 11.195064000000048) Heading: 3.1415926535897931

Updating self Position: (120, 12.312664000000048) Heading: 3.1415926535897931

Updating self Position: (120, 13.419088000000048) Heading: 3.1415926535897931

Updating self Position: (120, 14.525512000000049) Heading: 3.1415926535897931

Updating self Position: (120, 15.643112000000048) Heading: 3.1415926535897931

Updating self Position: (120, 16.749536000000049) Heading: 3.1415926535897931

Updating self Position: (120, 17.855960000000049) Heading: 3.1415926535897931

Updating self Position: (120, 18.96238400000005) Heading: 3.1415926535897931

Updating self Position: (120, 20.079984000000049) Heading: 3.1415926535897931

Updating self Position: (120, 21.197584000000049) Heading: 3.1415926535897931

Updating self Position: (120, 22.304008000000049) Heading: 3.1415926535897931

Updating self Position: (120, 23.41043200000005) Heading: 3.1415926535897931

Updating self Position: (120, 24.528032000000049) Heading: 3.1415926535897931

Updating self Position: (120, 25.63445600000005) Heading: 3.1415926535897931

Updating self Position: (120, 26.752056000000049) Heading: 3.1415926535897931

Updating self Position: (120, 27.869656000000049) Heading: 3.1415926535897931

Updating self Position: (120, 28.976080000000049) Heading: 3.1415926535897931

Updating self Position: (120, 30.08250400000005) Heading: 3.1415926535897931

Updating self Position: (120, 31.18892800000005) Heading: 3.1415926535897931

Updating self Position: (120, 32.30652800000005) Heading: 3.1415926535897931

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966

Updating self Position: (120, 32.30652800000005) Heading: 1.5707963267948966