feat: create new waypoint csv file for tracking original loop with tr…

…affic.

data/churchlot_rosbag_based.csv

@@ -0,0 +1,70 @@
+-1.63705,15.4211,0.3296619653701782,0.4356868560751291
+-0.948726,15.7415,0.3687630891799927,0.40911790242350793
+0.0183764,16.2415,0.457584023475647,0.3567155481927056
+1.04283,16.6584,0.5300092697143555,0.31441576296793283
+1.96748,16.9406,0.5682389736175537,0.276949103994321
+2.92915,17.2831,0.6166431903839111,0.2478796710128828
+3.8737,17.5936,0.6920894384384155,0.24784436855103026
+4.8254,17.8306,0.7460726499557495,0.23091005291957417
+5.78062,18.0927,0.8190367221832275,0.21523611978982907
+6.69051,18.3232,0.8448194265365601,0.20400538730834905
+7.77731,18.5681,0.9332854747772217,0.19071738353679682
+8.6967,18.8126,1.0074732303619385,0.17698070584870523
+9.69977,18.9788,1.0826950073242188,0.08401309830964404
+10.6113,19.1846,1.1239731311798096,0.12400767143948388
+11.7092,19.3419,1.1960995197296143,0.08044284530713439
+12.5403,19.4313,1.2527379989624023,0.021498974433084142
+13.5046,19.5588,1.294101357460022,-0.07173752282424099
+14.5204,19.5779,1.3653385639190674,-0.17911410597131802
+15.6525,19.3538,1.4690730571746826,-0.3017433806785146
+16.6725,19.1001,1.5444988012313843,-0.4288003847304158
+17.6537,18.631,1.6866918802261353,-0.5600816119091466
+18.3506,18.2821,1.7560275793075562,-0.6686755648229217
+19.0874,17.5749,1.8165712356567383,-0.6515137404887572
+19.7902,17.083,1.863821029663086,-0.7469733083804228
+20.6518,16.2624,1.9657000303268433,-0.8740797582359471
+20.9306,15.0283,1.997053623199463,-1.2431687626984453
+21.1768,14.4618,2.0402112007141113,-1.3228595109993015
+21.3902,13.4633,2.0778658390045166,-1.4528050830294885
+21.5045,12.4346,2.0919837951660156,-1.5667207065629973
+21.6505,11.6189,2.1050355434417725,-1.650443183031877
+21.5603,10.4305,2.069476366043091,-1.7626215933742448
+21.4396,9.73487,2.0802505016326904,-1.8235899631097143
+21.2035,8.77184,2.0931906700134277,-1.9153761859410117
+20.8137,7.79431,2.0673255920410156,-2.0143919953380616
+20.3767,6.89682,2.036637783050537,-2.1138426437467888
+19.8189,5.89824,2.0200588703155518,-2.2192511292545802
+19.4295,5.19827,1.996590256690979,-2.294310651868228
+18.6106,4.26401,1.974968433380127,-2.4042349136745
+18.0002,3.6869,1.943932056427002,-2.4723483767980063
+17.1655,2.9719,1.8803926706314087,-2.55744268736848
+16.2486,2.39129,1.8405542373657227,-2.62638007762677
+15.5798,2.02108,1.772679328918457,-2.6669456104545435
+14.6049,1.51789,1.7325119972229004,-2.7190172313822654
+13.5992,0.971969,1.68039870262146,-2.7717553107109603
+12.8759,0.660633,1.6190104484558105,-2.8050892059375747
+11.7323,0.21987,1.5723912715911865,-2.858018878754266
+10.6958,-0.0972202,1.5352537631988525,-2.9024230754721474
+9.61102,-0.347127,1.480985164642334,-2.956258498163751
+8.84021,-0.516434,1.4282913208007812,-2.9927807130825506
+7.75368,-0.650014,1.355726957321167,-3.0517189158287725
+7.04865,-0.777986,1.3189254999160767,-3.0928483236697555
+5.9447,-0.908794,1.2787668704986572,3.128476874135905
+4.78805,-0.956996,1.203260064125061,3.0578342937066214
+4.06115,-0.921798,1.1763063669204712,3.0112206647854265
+2.85048,-0.724293,1.0750387907028198,2.7674201281533883
+2.1127,-0.509599,1.0390881299972534,2.701484349111312
+1.02899,-0.0583726,0.9777755737304688,2.59225635356698
+0.0921522,0.480181,0.917380690574646,2.4883626797728584
+-0.485009,0.875734,0.8650240302085876,2.414661116116501
+-1.31595,1.59809,0.788451075553894,2.3003573738117034
+-1.95017,2.33911,0.7493326663970947,2.1807587770992876
+-2.65432,3.06567,0.6512490510940552,2.054049249826651
+-3.29638,3.90082,0.6531713604927063,1.9223537054549447
+-3.64865,4.70156,0.6156972646713257,1.836543777489471
+-3.99915,5.76614,0.5727978348731995,1.7079246962416879
+-4.17792,6.84398,0.49976247549057007,1.5782779561009748
+-4.22186,7.91132,0.4811903238296509,1.4591715027860204
+-4.11718,8.86346,0.43684834241867065,1.336328270825061
+-3.93796,9.79756,0.3884011507034302,1.2134568824181913
+-3.53336,11.502,0.34696757793426514,1.0169572295236207

ros/launch/site-rosbag.launch

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<launch>
+    <!--DBW Node -->
+    <include file="$(find twist_controller)/launch/dbw.launch"/>
+
+    <!--Waypoint Loader -->
+    <include file="$(find waypoint_loader)/launch/waypoint_loader_site_rosbag.launch"/>
+
+    <!--Waypoint Follower Node -->
+    <include file="$(find waypoint_follower)/launch/pure_pursuit.launch"/>
+
+    <!--Waypoint Updater Node -->
+    <include file="$(find waypoint_updater)/launch/waypoint_updater.launch"/>
+
+    <!--Traffic Light Locations and Camera Config -->
+    <param name="traffic_light_config" textfile="$(find tl_detector)/site_rosbag_traffic_light_config.yaml" />
+
+    <!--Traffic Light Detector Node -->
+    <include file="$(find tl_detector)/launch/tl_detector_site.launch"/>
+
+    <!--Camera Calibration YAML -->
+    <param name="grasshopper_calibration_yaml" textfile="$(find styx)../../../data/grasshopper_calibration.yml" />
+
+    <!--Camera Info Publisher -->
+    <include file="$(find camera_info_publisher)/launch/camera_info_publisher.launch"/>
+</launch>

ros/src/tl_detector/site_rosbag_traffic_light_config.yaml

@@ -0,0 +1,7 @@
+camera_info:
+  focal_length_x: 1345.200806
+  focal_length_y: 1353.838257
+  image_width: 800
+  image_height: 600
+light_positions:
+    - [14.607, 19.602]

ros/src/tools/diagScreenRosbag.py

@@ -356,15 +356,11 @@ def drawFinalWaypoints(self, img, size=1, size2=15):
 
     def drawTrafficLights(self, img, size=10):
         font = cv2.FONT_HERSHEY_COMPLEX
-        for i in range(len(self.lights)):
-            x = self.lights[i].pose.pose.position.x
-            y = self.lights[i].pose.pose.position.y
-            if self.lights[i].state == 0:
-                color = (255, 0, 0)
-            elif self.lights[i].state == 1:
-                color = (255, 255, 0)
-            else:
-                color = (0, 255, 0)
+        for i in range(len(self.traffic_light_to_waypoint_map)):
+            tlidx = self.traffic_light_to_waypoint_map[i]
+            x = self.waypoints[tlidx].pose.pose.position.x
+            y = self.waypoints[tlidx].pose.pose.position.y
+            color = (255, 255, 0)
             cv2.circle(img, (int(x*75+510), int(self.img_rows-(y*75+212))), size, color, -1)
             cv2.putText(img, "%d"%(i), (int(x*75+510-10), int(self.img_rows-(y*75+212)+40)), font, 1, color, 2)
 
@@ -418,7 +414,7 @@ def update_pygame(self):
         ### initialize pygame
         if self.screen is None:
             pygame.init()
-            pygame.display.set_caption("Udacity SDC System Integration Project: Vehicle Diagnostics")
+            pygame.display.set_caption("Udacity SDC System Integration Project: Rosbag Diagnostics")
             self.screen = pygame.display.set_mode((self.img_cols//self.img_vis_ratio,self.img_rows//self.img_vis_ratio), pygame.DOUBLEBUF)
         ## give us a machine view of the world
         self.sim_img = pygame.image.fromstring(cv2.resize(self.cv_image,(self.img_cols//self.img_vis_ratio, self.img_rows//self.img_vis_ratio),
@@ -434,7 +430,7 @@ def update_pygame(self):
     parser.add_argument('--textspacing', type=int, default="100", help='Text Spacing: default=100')
     parser.add_argument('--fontsize', type=float, default="2", help='Font Size: default=2')
     parser.add_argument('--cameratopic', type=str, default='/image_color', help='camera ros topic')
-    parser.add_argument('--trafficconfig', type=str, default='sim_traffic_light_config.yaml', help='traffic light yaml config')
+    parser.add_argument('--trafficconfig', type=str, default='site_rosbag_traffic_light_config.yaml', help='traffic light yaml config')
     args = parser.parse_args()
 
     try:

ros/src/waypoint_loader/launch/waypoint_loader_site_rosbag.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="waypoint_loader" type="waypoint_loader.py" name="waypoint_loader">
+        <param name="path" value="$(find styx)../../../data/churchlot_rosbag_based.csv" />
+        <param name="velocity" value="10" />
+    </node>
+</launch>