From ef49d55f5d37c9d11d3c3b3c2583aa06c783212f Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 14 Jun 2021 20:26:06 -0400 Subject: [PATCH 01/47] added ROSCameraPublisher.py and visualizer operator can publish camera frame data to foxglove-studio --- pylot/debug/ROSCameraPublisher.py | 23 +++++++++++++++++ ...zer_operator.py => visualizer_operater.py} | 25 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 pylot/debug/ROSCameraPublisher.py rename pylot/debug/{visualizer_operator.py => visualizer_operater.py} (93%) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py new file mode 100644 index 00000000..fbc41256 --- /dev/null +++ b/pylot/debug/ROSCameraPublisher.py @@ -0,0 +1,23 @@ +import sys +import rospy +import numpy as np +from sensor_msgs.msg import Image + +class ROSCameraPublisher: + + def __init__(self, topic:str): + # publishes to the given topic + self.image_pub = rospy.Publisher(topic, Image, queue_size=10) + + def publish(self, img_arr): + # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype + img_msg = Image(encoding='rgb8') + img_msg.height, img_msg.weight, channels = img_arr.shape + contig_arr = np.ascontiguousarray(img_arr) + img_msg.data = contig_arr.tobytes() + img_msg.step = img_msg.width * img_msg.height + img_msg.is_bigendian = ( + img_arr.dtype.byteorder == '>' or + img_arr.dtype.byteorder == '=' and sys.byteorder == 'big' + ) + self.image_pub.publish(img_msg) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operater.py similarity index 93% rename from pylot/debug/visualizer_operator.py rename to pylot/debug/visualizer_operater.py index 6882ea1b..79347fd5 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operater.py @@ -19,6 +19,8 @@ DEFAULT_VIS_TIME = 30000.0 +import rospy +from .ROSCameraPublisher import ROSCameraPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state @@ -141,6 +143,10 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, mono = pygame.font.match_font(mono) self.font = pygame.font.Font(mono, 14) + # PYLOT-ROS Integration + rospy.init_node("visualizer", anonymous=True, disable_signals=True) + self.pub = {} # dict of publishers + # Array of keys to figure out which message to display. self.current_display = 0 self.display_array = [] @@ -148,27 +154,35 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_rgb_camera: self.display_array.append("RGB") self.window_titles.append("RGB Camera") + self.pub["RBG"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: self.display_array.append("Obstacle") self.window_titles.append("Detected obstacles") + self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: self.display_array.append("TrackedObstacle") self.window_titles.append("Obstacle tracking") + self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: self.display_array.append("TLCamera") self.window_titles.append("Detected traffic lights") + self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: self.display_array.append("Waypoint") self.window_titles.append("Planning") + self.pub["Waypoint"] = ROSCameraPublisher("/camera/waypoint") if flags.visualize_prediction: self.display_array.append("PredictionCamera") self.window_titles.append("Prediction") + # self.pub["PredictionCamera"] = ROSCameraPublisher("/camera/prediction_camera") if flags.visualize_lidar: self.display_array.append("PointCloud") self.window_titles.append("LiDAR") + # self.pub["Lanes"] = ROSPointCloudPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: self.display_array.append("Lanes") self.window_titles.append("Detected lanes") + self.pub["Lanes"] = ROSCameraPublisher("/camera/lanes") if flags.visualize_depth_camera: self.display_array.append("Depth") self.window_titles.append("Depth Camera") @@ -318,22 +332,26 @@ def on_watermark(self, timestamp): sensor_to_display = self.display_array[self.current_display] if sensor_to_display == "RGB" and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif (sensor_to_display == "TLCamera" and tl_camera_msg and traffic_light_msg): tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, tl_camera_msg.frame) elif (sensor_to_display == "TrackedObstacle" and bgr_msg and tracked_obstacle_msg): bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) bgr_msg.frame.visualize(self.display) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg and waypoint_msg): bgr_frame = bgr_msg.frame @@ -344,6 +362,7 @@ def on_watermark(self, timestamp): if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) bgr_frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_frame) elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg and prediction_msg): frame = prediction_camera_msg.frame @@ -359,6 +378,7 @@ def on_watermark(self, timestamp): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) bgr_msg.frame.visualize(self.display, timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Depth" and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) elif sensor_to_display == "Segmentation" and segmentation_msg: @@ -431,3 +451,8 @@ def _visualize_imu(self, msg): end_acc.as_simulator_location(), arrow_size=0.1, life_time=0.1) + + def _publish_camera_frame(self, sensor_to_display, frame): + image_np = frame.as_rgb_numpy_array() + self.pub[sensor_to_display].publish(image_np) + From a4f092bbf05a1d415e11b8a62e14e4f7f714728b Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 14 Jun 2021 20:27:47 -0400 Subject: [PATCH 02/47] minimal name changes --- pylot/debug/{visualizer_operater.py => visualizer_operator.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pylot/debug/{visualizer_operater.py => visualizer_operator.py} (100%) diff --git a/pylot/debug/visualizer_operater.py b/pylot/debug/visualizer_operator.py similarity index 100% rename from pylot/debug/visualizer_operater.py rename to pylot/debug/visualizer_operator.py From a7ecf65be053b76615c8768e6f59a578b4d5214e Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 15 Jun 2021 03:43:51 -0400 Subject: [PATCH 03/47] fixed variable typo --- pylot/debug/ROSCameraPublisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index fbc41256..9c02553a 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -12,7 +12,7 @@ def __init__(self, topic:str): def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype img_msg = Image(encoding='rgb8') - img_msg.height, img_msg.weight, channels = img_arr.shape + img_msg.height, img_msg.width, channels = img_arr.shape contig_arr = np.ascontiguousarray(img_arr) img_msg.data = contig_arr.tobytes() img_msg.step = img_msg.width * img_msg.height From 44ff20539fdf21a4a47461b47d234a2a0e4637fa Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 17 Jun 2021 01:07:38 -0400 Subject: [PATCH 04/47] visualizer operator can now publish ROS PointCloud2 messages (displayed in foxglove-studio) --- pylot/debug/ROSCameraPublisher.py | 3 +-- pylot/debug/ROSLIDARPublisher.py | 32 ++++++++++++++++++++++++++++++ pylot/debug/visualizer_operator.py | 5 ++++- 3 files changed, 37 insertions(+), 3 deletions(-) create mode 100644 pylot/debug/ROSLIDARPublisher.py diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index 9c02553a..bff1d55a 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -13,8 +13,7 @@ def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype img_msg = Image(encoding='rgb8') img_msg.height, img_msg.width, channels = img_arr.shape - contig_arr = np.ascontiguousarray(img_arr) - img_msg.data = contig_arr.tobytes() + img_msg.data = img_arr.tobytes() img_msg.step = img_msg.width * img_msg.height img_msg.is_bigendian = ( img_arr.dtype.byteorder == '>' or diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py new file mode 100644 index 00000000..9200dbc2 --- /dev/null +++ b/pylot/debug/ROSLIDARPublisher.py @@ -0,0 +1,32 @@ +import sys +import rospy +import numpy as np +from sensor_msgs.msg import PointCloud2, PointField + +class ROSLIDARPublisher: + + def __init__(self, topic:str): + # publishes to the given topic + self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) + + def publish(self, points): + # converts point cloud points array to a sensor_msgs/PointCloud2 datatype + points = points.astype(np.float32) + points[:,[0,2]] = points[:,[2,0]] + points[:,[1,2]] = points[:,[2,1]] + points[:,2] = -points[:,2] + points_byte_array = points.tobytes() + row_step = len(points_byte_array) + point_step = len(points[0].tobytes()) + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + point_cloud_msg = PointCloud2(height=1, + width=len(points), + is_dense=True, + is_bigendian=False, + fields=fields, + point_step=point_step, + row_step=row_step, + data=points_byte_array) + self.point_cloud_pub.publish(point_cloud_msg) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 79347fd5..0d745906 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -19,8 +19,10 @@ DEFAULT_VIS_TIME = 30000.0 +# PYLOT-ROS Integration import rospy from .ROSCameraPublisher import ROSCameraPublisher +from .ROSLIDARPublisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state @@ -178,7 +180,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_lidar: self.display_array.append("PointCloud") self.window_titles.append("LiDAR") - # self.pub["Lanes"] = ROSPointCloudPublisher("/lidar/point_cloud") + self.pub["PointCloud"] = ROSLIDARPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: self.display_array.append("Lanes") self.window_titles.append("Detected lanes") @@ -374,6 +376,7 @@ def on_watermark(self, timestamp): point_cloud_msg.point_cloud.visualize( self.display, self._flags.camera_image_width, self._flags.camera_image_height) + self.pub["PointCloud"].publish(point_cloud_msg.point_cloud.points) elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) From 83c3b9857a9002dd5e2dff200ac8993d3269feff Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 17 Jun 2021 16:07:14 -0400 Subject: [PATCH 05/47] fixed mirror image for lidar --- pylot/debug/ROSLIDARPublisher.py | 1 + 1 file changed, 1 insertion(+) diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py index 9200dbc2..97bcf0d1 100644 --- a/pylot/debug/ROSLIDARPublisher.py +++ b/pylot/debug/ROSLIDARPublisher.py @@ -14,6 +14,7 @@ def publish(self, points): points = points.astype(np.float32) points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] + points[:,0] = -points[:,0] points[:,2] = -points[:,2] points_byte_array = points.tobytes() row_step = len(points_byte_array) From cd44d2c4ae60719a7ccbce9675716c2ef7c319cb Mon Sep 17 00:00:00 2001 From: Josh Date: Wed, 23 Jun 2021 04:23:21 -0400 Subject: [PATCH 06/47] visualizer operator can visualize segmentation frames and depth frames (both as rgb frame and point cloud), also cleaned up some code --- pylot/debug/ROSCameraPublisher.py | 3 ++ pylot/debug/ROSLIDARPublisher.py | 4 --- pylot/debug/visualizer_operator.py | 45 ++++++++++++++++++++---------- 3 files changed, 34 insertions(+), 18 deletions(-) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index bff1d55a..84a49256 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -11,7 +11,10 @@ def __init__(self, topic:str): def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype + # inspired by https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py img_msg = Image(encoding='rgb8') + if type(img_arr[0][0][0]) != np.int8: + img_arr = img_arr.astype(np.int8) img_msg.height, img_msg.width, channels = img_arr.shape img_msg.data = img_arr.tobytes() img_msg.step = img_msg.width * img_msg.height diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py index 97bcf0d1..ef68c126 100644 --- a/pylot/debug/ROSLIDARPublisher.py +++ b/pylot/debug/ROSLIDARPublisher.py @@ -12,10 +12,6 @@ def __init__(self, topic:str): def publish(self, points): # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) - points[:,[0,2]] = points[:,[2,0]] - points[:,[1,2]] = points[:,[2,1]] - points[:,0] = -points[:,0] - points[:,2] = -points[:,2] points_byte_array = points.tobytes() row_step = len(points_byte_array) point_step = len(points[0].tobytes()) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 0d745906..c6abe98c 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -19,7 +19,6 @@ DEFAULT_VIS_TIME = 30000.0 -# PYLOT-ROS Integration import rospy from .ROSCameraPublisher import ROSCameraPublisher from .ROSLIDARPublisher import ROSLIDARPublisher @@ -156,7 +155,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_rgb_camera: self.display_array.append("RGB") self.window_titles.append("RGB Camera") - self.pub["RBG"] = ROSCameraPublisher("/camera/rgb") + self.pub["RGB"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: self.display_array.append("Obstacle") self.window_titles.append("Detected obstacles") @@ -188,9 +187,13 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_depth_camera: self.display_array.append("Depth") self.window_titles.append("Depth Camera") + #self.pub["Depth"] = ROSLIDARPublisher("/lidar/depth") # need to create a flag called --visualize_depth_point_cloud + self.pub["Depth"] = ROSCameraPublisher("/camera/depth") + self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: self.display_array.append("Segmentation") self.window_titles.append("Segmentation") + self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) top_down_transform = pylot.utils.get_top_down_transform( @@ -334,26 +337,30 @@ def on_watermark(self, timestamp): sensor_to_display = self.display_array[self.current_display] if sensor_to_display == "RGB" and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["RGB"].publish(image_np) elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["Obstacle"].publish(image_np) elif (sensor_to_display == "TLCamera" and tl_camera_msg and traffic_light_msg): tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, tl_camera_msg.frame) + image_np = tl_camera_msg.frame.as_rgb_numpy_array() + self.pub["TLCamera"].publish(image_np) elif (sensor_to_display == "TrackedObstacle" and bgr_msg and tracked_obstacle_msg): bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) bgr_msg.frame.visualize(self.display) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["TrackedObstacle"].publish(image_np) elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg and waypoint_msg): bgr_frame = bgr_msg.frame @@ -364,7 +371,8 @@ def on_watermark(self, timestamp): if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) bgr_frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_frame) + image_np = bgr_frame.as_rgb_numpy_array() + self.pub["Waypoint"].publish(image_np) elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg and prediction_msg): frame = prediction_camera_msg.frame @@ -376,16 +384,30 @@ def on_watermark(self, timestamp): point_cloud_msg.point_cloud.visualize( self.display, self._flags.camera_image_width, self._flags.camera_image_height) - self.pub["PointCloud"].publish(point_cloud_msg.point_cloud.points) + points = point_cloud_msg.point_cloud.points + points[:,[0,2]] = points[:,[2,0]] + points[:,[1,2]] = points[:,[2,1]] + points[:,0] = -points[:,0] + points[:,2] = -points[:,2] + self.pub["PointCloud"].publish(points) elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) bgr_msg.frame.visualize(self.display, timestamp) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["Lanes"].publish(image_np) elif sensor_to_display == "Depth" and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) + image_np = depth_msg.frame.original_frame + image_np = image_np[:, :, ::-1] + self.pub["Depth"].publish(image_np) + depth_msg.frame.resize(854, 480) + points = depth_msg.frame.as_point_cloud() + points[:,0] = -points[:,0] + self.pub["DepthPointCloud"].publish(points) elif sensor_to_display == "Segmentation" and segmentation_msg: segmentation_msg.frame.visualize(self.display, timestamp=timestamp) + self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) elif sensor_to_display == "PlanningWorld": if prediction_camera_msg is None: # Top-down prediction is not available. Show planning @@ -454,8 +476,3 @@ def _visualize_imu(self, msg): end_acc.as_simulator_location(), arrow_size=0.1, life_time=0.1) - - def _publish_camera_frame(self, sensor_to_display, frame): - image_np = frame.as_rgb_numpy_array() - self.pub[sensor_to_display].publish(image_np) - From 8155d82ecaceeefa77ff785ad853cdc8dcc86428 Mon Sep 17 00:00:00 2001 From: Josh Date: Wed, 23 Jun 2021 22:04:38 -0400 Subject: [PATCH 07/47] visualizes planning world --- pylot/debug/visualizer_operator.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index c6abe98c..f1a47920 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -205,6 +205,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, flags.camera_image_height, top_down_transform, 90) self.display_array.append("PlanningWorld") self.window_titles.append("Planning world") + self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") else: self._planning_world = None assert len(self.display_array) == len(self.window_titles), \ @@ -433,6 +434,8 @@ def on_watermark(self, timestamp): self._planning_world.update_waypoints(None, waypoint_msg.waypoints) self._planning_world.draw_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) + image_np = frame.as_rgb_numpy_array() + self.pub["PlanningWorld"].publish(image_np) self.render_text(pose_msg.data, control_msg, timestamp) From bf5a24db4826800461226e8280543695137fdd56 Mon Sep 17 00:00:00 2001 From: Josh Date: Sat, 3 Jul 2021 19:48:01 -0400 Subject: [PATCH 08/47] streams can be displayed simultaneously (in foxglove), still has pygame code to be removed --- pylot/debug/visualizer_operator.py | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index f1a47920..e1763ad0 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -336,34 +336,31 @@ def on_watermark(self, timestamp): self._visualize_imu(imu_msg) sensor_to_display = self.display_array[self.current_display] - if sensor_to_display == "RGB" and bgr_msg: + if self._flags.visualize_rgb_camera and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["RGB"].publish(image_np) - elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: + if self._flags.visualize_detected_obstacles and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Obstacle"].publish(image_np) - elif (sensor_to_display == "TLCamera" and tl_camera_msg - and traffic_light_msg): + if self._flags.visualize_detected_traffic_lights and tl_camera_msg and traffic_light_msg: tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) image_np = tl_camera_msg.frame.as_rgb_numpy_array() self.pub["TLCamera"].publish(image_np) - elif (sensor_to_display == "TrackedObstacle" and bgr_msg - and tracked_obstacle_msg): + if self._flags.visualize_tracked_obstacles and bgr_msg and tracked_obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) bgr_msg.frame.visualize(self.display) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["TrackedObstacle"].publish(image_np) - elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg - and waypoint_msg): + if self._flags.visualize_waypoints and bgr_msg and pose_msg and waypoint_msg: bgr_frame = bgr_msg.frame if self._flags.draw_waypoints_on_camera_frames: bgr_frame.camera_setup.set_transform( @@ -374,14 +371,13 @@ def on_watermark(self, timestamp): bgr_frame.visualize(self.display, timestamp=timestamp) image_np = bgr_frame.as_rgb_numpy_array() self.pub["Waypoint"].publish(image_np) - elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg - and prediction_msg): + if prediction_camera_msg and prediction_msg: frame = prediction_camera_msg.frame frame.transform_to_cityscapes() for obstacle_prediction in prediction_msg.predictions: obstacle_prediction.draw_trajectory_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) - elif sensor_to_display == "PointCloud" and point_cloud_msg: + if self._flags.visualize_lidar and point_cloud_msg: point_cloud_msg.point_cloud.visualize( self.display, self._flags.camera_image_width, self._flags.camera_image_height) @@ -391,13 +387,13 @@ def on_watermark(self, timestamp): points[:,0] = -points[:,0] points[:,2] = -points[:,2] self.pub["PointCloud"].publish(points) - elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): + if self._flags.visualize_detected_lanes and bgr_msg and lane_detection_msg: for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) bgr_msg.frame.visualize(self.display, timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Lanes"].publish(image_np) - elif sensor_to_display == "Depth" and depth_msg: + if self._flags.visualize_depth_camera and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) image_np = depth_msg.frame.original_frame image_np = image_np[:, :, ::-1] @@ -406,16 +402,16 @@ def on_watermark(self, timestamp): points = depth_msg.frame.as_point_cloud() points[:,0] = -points[:,0] self.pub["DepthPointCloud"].publish(points) - elif sensor_to_display == "Segmentation" and segmentation_msg: + if self._flags.visualize_segmentation and segmentation_msg: segmentation_msg.frame.visualize(self.display, timestamp=timestamp) self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) - elif sensor_to_display == "PlanningWorld": + if self._flags.visualize_world: if prediction_camera_msg is None: # Top-down prediction is not available. Show planning # world on a black image. black_img = np.zeros((self._bird_eye_camera_setup.height, - self._bird_eye_camera_setup.width, 3), - dtype=np.dtype("uint8")) + self._bird_eye_camera_setup.width, 3), + dtype=np.dtype("uint8")) frame = CameraFrame(black_img, 'RGB', self._bird_eye_camera_setup) else: From ccce97715f31baeb178a632bc9f62a20ba719ce5 Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 4 Jul 2021 02:43:15 -0400 Subject: [PATCH 09/47] removed all dependencies on pygame in visualizer_operator, though still some pygame code in other files --- pylot/debug/visualizer_operator.py | 90 +++--------------------------- 1 file changed, 7 insertions(+), 83 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index e1763ad0..5e1cd4b2 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -9,9 +9,6 @@ import numpy as np -import pygame -from pygame.locals import K_n - import pylot.utils from pylot.drivers.sensor_setup import RGBCameraSetup from pylot.perception.camera_frame import CameraFrame @@ -25,10 +22,10 @@ class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state - of the entire pipeline by visualizing it on a pygame instance. + of the entire pipeline by visualizing it in Foxglove Studio. This receives input data from almost the entire pipeline and renders the - results of the operator currently chosen by the developer on the screen. + results of the operator(s) in Foxglove Studio. """ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, prediction_camera_stream, depth_camera_stream, @@ -36,7 +33,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, obstacles_stream, traffic_lights_stream, tracked_obstacles_stream, lane_detection_stream, prediction_stream, waypoints_stream, control_stream, - display_control_stream, pygame_display, flags): + control_display_stream, flags): visualize_streams = [] self._pose_msgs = deque() pose_stream.add_callback( @@ -132,67 +129,32 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, erdos.add_watermark_callback(visualize_streams, [], self.on_watermark) # Add a callback on a control stream to figure out what to display. - display_control_stream.add_callback(self.change_display) self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) - self.display = pygame_display - - # Set the font. - fonts = [x for x in pygame.font.get_fonts() if 'mono' in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self.font = pygame.font.Font(mono, 14) # PYLOT-ROS Integration rospy.init_node("visualizer", anonymous=True, disable_signals=True) self.pub = {} # dict of publishers - # Array of keys to figure out which message to display. - self.current_display = 0 - self.display_array = [] - self.window_titles = [] + # Creating ROS publishers for streams to be visualized if flags.visualize_rgb_camera: - self.display_array.append("RGB") - self.window_titles.append("RGB Camera") self.pub["RGB"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: - self.display_array.append("Obstacle") - self.window_titles.append("Detected obstacles") self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: - self.display_array.append("TrackedObstacle") - self.window_titles.append("Obstacle tracking") self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: - self.display_array.append("TLCamera") - self.window_titles.append("Detected traffic lights") self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: - self.display_array.append("Waypoint") - self.window_titles.append("Planning") self.pub["Waypoint"] = ROSCameraPublisher("/camera/waypoint") - if flags.visualize_prediction: - self.display_array.append("PredictionCamera") - self.window_titles.append("Prediction") - # self.pub["PredictionCamera"] = ROSCameraPublisher("/camera/prediction_camera") if flags.visualize_lidar: - self.display_array.append("PointCloud") - self.window_titles.append("LiDAR") self.pub["PointCloud"] = ROSLIDARPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: - self.display_array.append("Lanes") - self.window_titles.append("Detected lanes") self.pub["Lanes"] = ROSCameraPublisher("/camera/lanes") if flags.visualize_depth_camera: - self.display_array.append("Depth") - self.window_titles.append("Depth Camera") - #self.pub["Depth"] = ROSLIDARPublisher("/lidar/depth") # need to create a flag called --visualize_depth_point_cloud self.pub["Depth"] = ROSCameraPublisher("/camera/depth") self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: - self.display_array.append("Segmentation") - self.window_titles.append("Segmentation") self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) @@ -203,13 +165,9 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._bird_eye_camera_setup = RGBCameraSetup( 'bird_eye_camera', flags.camera_image_width, flags.camera_image_height, top_down_transform, 90) - self.display_array.append("PlanningWorld") - self.window_titles.append("Planning world") self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") else: self._planning_world = None - assert len(self.display_array) == len(self.window_titles), \ - "The display and titles differ." # Save the flags. self._flags = flags @@ -231,13 +189,6 @@ def save(self, msg, msg_type, queue): msg.timestamp, msg_type)) queue.append(msg) - def change_display(self, display_message): - if display_message.data == K_n: - self.current_display = (self.current_display + 1) % len( - self.display_array) - self._logger.debug("@{}: Visualizer changed to {}".format( - display_message.timestamp, self.current_display)) - def get_message(self, queue, timestamp, name): msg = None if queue: @@ -278,21 +229,7 @@ def render_text(self, pose, control, timestamp): "Reverse : {:.2f}".format(control.reverse), ] - # Display the information box. - info_surface = pygame.Surface( - (220, self._flags.camera_image_height // 3)) - info_surface.set_alpha(100) - self.display.blit(info_surface, (0, 0)) - - # Render the text. - v_offset = 10 - for line in info_text: - if v_offset + 18 > self._flags.camera_image_height: - break - surface = self.font.render(line, True, (255, 255, 255)) - self.display.blit(surface, (8, v_offset)) - v_offset += 18 - pygame.display.flip() + # TODO: Render text (using ROS publisher) def on_watermark(self, timestamp): self._logger.debug("@{}: received watermark.".format(timestamp)) @@ -335,29 +272,24 @@ def on_watermark(self, timestamp): if self._flags.visualize_imu: self._visualize_imu(imu_msg) - sensor_to_display = self.display_array[self.current_display] if self._flags.visualize_rgb_camera and bgr_msg: - bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["RGB"].publish(image_np) if self._flags.visualize_detected_obstacles and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) - bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Obstacle"].publish(image_np) if self._flags.visualize_detected_traffic_lights and tl_camera_msg and traffic_light_msg: tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) - tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) image_np = tl_camera_msg.frame.as_rgb_numpy_array() self.pub["TLCamera"].publish(image_np) if self._flags.visualize_tracked_obstacles and bgr_msg and tracked_obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) - bgr_msg.frame.visualize(self.display) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["TrackedObstacle"].publish(image_np) if self._flags.visualize_waypoints and bgr_msg and pose_msg and waypoint_msg: @@ -368,7 +300,6 @@ def on_watermark(self, timestamp): waypoint_msg.waypoints.draw_on_frame(bgr_frame) if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) - bgr_frame.visualize(self.display, timestamp=timestamp) image_np = bgr_frame.as_rgb_numpy_array() self.pub["Waypoint"].publish(image_np) if prediction_camera_msg and prediction_msg: @@ -376,11 +307,7 @@ def on_watermark(self, timestamp): frame.transform_to_cityscapes() for obstacle_prediction in prediction_msg.predictions: obstacle_prediction.draw_trajectory_on_frame(frame) - frame.visualize(self.display, timestamp=timestamp) if self._flags.visualize_lidar and point_cloud_msg: - point_cloud_msg.point_cloud.visualize( - self.display, self._flags.camera_image_width, - self._flags.camera_image_height) points = point_cloud_msg.point_cloud.points points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] @@ -390,11 +317,9 @@ def on_watermark(self, timestamp): if self._flags.visualize_detected_lanes and bgr_msg and lane_detection_msg: for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) - bgr_msg.frame.visualize(self.display, timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Lanes"].publish(image_np) if self._flags.visualize_depth_camera and depth_msg: - depth_msg.frame.visualize(self.display, timestamp=timestamp) image_np = depth_msg.frame.original_frame image_np = image_np[:, :, ::-1] self.pub["Depth"].publish(image_np) @@ -403,7 +328,6 @@ def on_watermark(self, timestamp): points[:,0] = -points[:,0] self.pub["DepthPointCloud"].publish(points) if self._flags.visualize_segmentation and segmentation_msg: - segmentation_msg.frame.visualize(self.display, timestamp=timestamp) self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) if self._flags.visualize_world: if prediction_camera_msg is None: @@ -429,11 +353,11 @@ def on_watermark(self, timestamp): lanes=lanes) self._planning_world.update_waypoints(None, waypoint_msg.waypoints) self._planning_world.draw_on_frame(frame) - frame.visualize(self.display, timestamp=timestamp) image_np = frame.as_rgb_numpy_array() self.pub["PlanningWorld"].publish(image_np) - self.render_text(pose_msg.data, control_msg, timestamp) + # todo: render text in foxglove + #self.render_text(pose_msg.data, control_msg, timestamp) def run(self): # Run method is invoked after all operators finished initializing. From 22a3900b25b4788d1cf0d6309808baa5c7df6647 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:21:19 -0400 Subject: [PATCH 10/47] removed run_visualizer_control_loop --- data_gatherer.py | 2 -- pylot.py | 2 -- pylot/utils.py | 27 --------------------------- 3 files changed, 31 deletions(-) diff --git a/data_gatherer.py b/data_gatherer.py index 3163e832..d9b31f4d 100644 --- a/data_gatherer.py +++ b/data_gatherer.py @@ -284,8 +284,6 @@ def main(argv): stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True))) try: - if pylot.flags.must_visualize(): - pylot.utils.run_visualizer_control_loop(control_display_stream) node_handle.wait() except KeyboardInterrupt: node_handle.shutdown() diff --git a/pylot.py b/pylot.py index 6c620d1d..de7ac93b 100644 --- a/pylot.py +++ b/pylot.py @@ -258,8 +258,6 @@ def main(args): client.start_recorder(FLAGS.simulation_recording_file) node_handle, control_display_stream = driver() signal.signal(signal.SIGINT, shutdown) - if pylot.flags.must_visualize(): - pylot.utils.run_visualizer_control_loop(control_display_stream) node_handle.wait() except KeyboardInterrupt: shutdown_pylot(node_handle, client, world) diff --git a/pylot/utils.py b/pylot/utils.py index d3774e13..cffe6c43 100644 --- a/pylot/utils.py +++ b/pylot/utils.py @@ -1049,33 +1049,6 @@ def set_tf_loglevel(level): logging.getLogger('tensorflow').setLevel(level) -def run_visualizer_control_loop(control_display_stream): - """Runs a pygame loop that waits for user commands. - - The user commands are send on the control_display_stream - to control the pygame visualization window. - """ - import erdos - import pygame - clock = pygame.time.Clock() - from pygame.locals import K_n - while True: - clock.tick_busy_loop(60) - events = pygame.event.get() - for event in events: - if event.type == pygame.KEYUP: - if event.key == K_n: - control_display_stream.send( - erdos.Message(erdos.Timestamp(coordinates=[0]), - event.key)) - elif event.type == pygame.QUIT: - raise KeyboardInterrupt - elif event.type == pygame.KEYDOWN: - if (event.key == pygame.K_c - and pygame.key.get_mods() & pygame.KMOD_CTRL): - raise KeyboardInterrupt - - def verify_keys_in_dict(required_keys, arg_dict): assert set(required_keys).issubset(set(arg_dict.keys())), \ "one or more of {} not found in {}".format(required_keys, arg_dict) From a7b36ce2797c758c839925217ede0e7233293828 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:26:09 -0400 Subject: [PATCH 11/47] removed visualize() methods from camera_frame.py, depth_frame.py, point_cloud.py, and segmented_frame.py --- pylot/perception/camera_frame.py | 13 ------------- pylot/perception/depth_frame.py | 10 ---------- pylot/perception/point_cloud.py | 18 ------------------ .../perception/segmentation/segmented_frame.py | 9 --------- 4 files changed, 50 deletions(-) diff --git a/pylot/perception/camera_frame.py b/pylot/perception/camera_frame.py index 3e761f7e..9bfee860 100644 --- a/pylot/perception/camera_frame.py +++ b/pylot/perception/camera_frame.py @@ -137,19 +137,6 @@ def resize(self, width: int, height: int): dsize=(width, height), interpolation=cv2.INTER_NEAREST) - def visualize(self, pygame_display, timestamp=None): - """Visualizes the frame on a pygame display.""" - import pygame - if timestamp is not None: - pylot.utils.add_timestamp(self.frame, timestamp) - if self.encoding != 'RGB': - image_np = self.as_rgb_numpy_array() - else: - image_np = self.frame - image_np = np.transpose(image_np, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def save(self, timestamp: int, data_path: str, file_base: str): """Saves the camera frame to a file. diff --git a/pylot/perception/depth_frame.py b/pylot/perception/depth_frame.py index bf49987d..084ba2ca 100644 --- a/pylot/perception/depth_frame.py +++ b/pylot/perception/depth_frame.py @@ -128,16 +128,6 @@ def resize(self, width: int, height: int): dsize=(width, height), interpolation=cv2.INTER_NEAREST) - def visualize(self, pygame_display, timestamp=None): - """Visualizes the frame on a pygame display.""" - if self.original_frame is not None: - import pygame - image_np = self.original_frame - image_np = image_np[:, :, ::-1] - image_np = np.transpose(image_np, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def save(self, timestamp: int, data_path: str, file_base: str): """Saves the depth frame to a file. diff --git a/pylot/perception/point_cloud.py b/pylot/perception/point_cloud.py index ed8fea61..3c426322 100644 --- a/pylot/perception/point_cloud.py +++ b/pylot/perception/point_cloud.py @@ -184,24 +184,6 @@ def save(self, timestamp: int, data_path: str, file_base: str): pcd.points = o3d.Vector3dVector(self.points) o3d.write_point_cloud(file_name, pcd) - def visualize(self, pygame_display, display_width: int, - display_height: int): - """Visualizes the point cloud on a pygame display.""" - import pygame - # Transform point cloud to top down view. - lidar_data = np.array(self.global_points[:, :2]) - lidar_data *= (min(display_width, display_height) / - (2.0 * self._lidar_setup.get_range_in_meters())) - lidar_data += (0.5 * display_width, 0.5 * display_height) - lidar_data = np.fabs(lidar_data) - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (display_width, display_height, 3) - lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - pygame.surfarray.blit_array(pygame_display, lidar_img) - pygame.display.flip() - def __repr__(self): return 'PointCloud(lidar setup: {}, points: {})'.format( self._lidar_setup, self.points) diff --git a/pylot/perception/segmentation/segmented_frame.py b/pylot/perception/segmentation/segmented_frame.py index 2d793fe2..f87cf71d 100644 --- a/pylot/perception/segmentation/segmented_frame.py +++ b/pylot/perception/segmentation/segmented_frame.py @@ -274,15 +274,6 @@ def save(self, timestamp, data_path, file_base): img = Image.fromarray(self.as_cityscapes_palette()) img.save(file_name) - def visualize(self, pygame_display, timestamp=None): - import pygame - cityscapes_frame = self.as_cityscapes_palette() - if timestamp is not None: - pylot.utils.add_timestamp(cityscapes_frame, timestamp) - image_np = np.transpose(cityscapes_frame, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def draw_box(self, start_point, end_point, color, thickness=3): """Draw a colored box defined by start_point, end_point.""" start = (int(start_point.x), int(start_point.y)) From 87b3a53179846bd169aa859cd5a0c44de03020ca Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:40:56 -0400 Subject: [PATCH 12/47] renamed ROSCameraPublisher.py to ros_camera_publisher.py and ROSLIDARPublisher.py to ros_lidar_publisher.py and adjusted import statement in visualizer operator --- .../debug/{ROSCameraPublisher.py => ros_camera_publisher.py} | 0 pylot/debug/{ROSLIDARPublisher.py => ros_lidar_publisher.py} | 0 pylot/debug/visualizer_operator.py | 4 ++-- 3 files changed, 2 insertions(+), 2 deletions(-) rename pylot/debug/{ROSCameraPublisher.py => ros_camera_publisher.py} (100%) rename pylot/debug/{ROSLIDARPublisher.py => ros_lidar_publisher.py} (100%) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ros_camera_publisher.py similarity index 100% rename from pylot/debug/ROSCameraPublisher.py rename to pylot/debug/ros_camera_publisher.py diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ros_lidar_publisher.py similarity index 100% rename from pylot/debug/ROSLIDARPublisher.py rename to pylot/debug/ros_lidar_publisher.py diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 5e1cd4b2..a7f3f351 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -17,8 +17,8 @@ DEFAULT_VIS_TIME = 30000.0 import rospy -from .ROSCameraPublisher import ROSCameraPublisher -from .ROSLIDARPublisher import ROSLIDARPublisher +from .ros_camera_publisher import ROSCameraPublisher +from .ros_lidar_publisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state From 03e97524d1d6e00ea987927f9dfac0187c9d4e24 Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 6 Jul 2021 02:17:56 -0400 Subject: [PATCH 13/47] added updated dockerfile (WIP) to install foxglove and ROS --- docker/Dockerfile | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index 80991608..acef0a74 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -72,3 +72,30 @@ RUN echo "if [ -f ~/.bashrc ]; then . ~/.bashrc ; fi" >> ~/.bash_profile # Set up ssh access to the container. RUN cd ~/ && ssh-keygen -q -t rsa -N '' -f ~/.ssh/id_rsa <<&1 >/dev/null RUN sudo sed -i 's/#X11UseLocalhost yes/X11UseLocalhost no/g' /etc/ssh/sshd_config + +# WIP (for visualization in Foxglove) +# FOR NOW: install foxglove-studio and ROS manually in the pylot container + +# # Download Foxglove +# RUN cd /home/erdos/workspace/pylot +# RUN curl -L https://github.com/foxglove/studio/releases/download/v0.10.2/foxglove-studio-0.10.2-linux-amd64.deb -o foxglove-studio-0.10.2-linux-amd64.deb +# RUN sudo apt install ./foxglove-studio-*.deb + +# # Download ROS +# RUN cd /home/erdos/workspace/pylot +# # From, http://wiki.ros.org/melodic/Installation/Ubuntu: +# # Setup your computer to accept software from packages.ros.org: +# RUN sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +# # Set up your keys +# RUN sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +# # Make sure your Debian package index is up-to-date +# RUN sudo apt update +# # Desktop-Full Install +# RUN sudo apt install ros-melodic-desktop-full +# # Set up ROS environment variables +# RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc +# RUN source ~/.bashrc + + + + From dd73dfc3ae4d4eac7e51201ab21c5f42298c9717 Mon Sep 17 00:00:00 2001 From: Ionel Gog Date: Tue, 6 Jul 2021 23:10:57 -0700 Subject: [PATCH 14/47] [Bug] Ensure localization sends a pose even if it is innacurate. --- .gitignore | 1 + pylot.py | 2 +- pylot/localization/localization_operator.py | 6 ++---- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index 2af531ab..f02ef2e3 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ *.log *.csv *.ipynb +*.dot dependencies/ pylot/prediction/prediction/ diff --git a/pylot.py b/pylot.py index 572712e8..1d7665e7 100644 --- a/pylot.py +++ b/pylot.py @@ -218,7 +218,7 @@ def driver(): prediction_stream, waypoints_stream, control_stream) streams_to_send_top_on += ingest_streams - node_handle = erdos.run_async() + node_handle = erdos.run_async('pylot.dot') for stream in streams_to_send_top_on: stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True))) diff --git a/pylot/localization/localization_operator.py b/pylot/localization/localization_operator.py index 66c7f5d2..9974aaa7 100644 --- a/pylot/localization/localization_operator.py +++ b/pylot/localization/localization_operator.py @@ -182,10 +182,8 @@ def on_watermark(self, timestamp: Timestamp): if pose_msg: self._last_pose_estimate = pose_msg.data self._last_timestamp = timestamp.coordinates[0] - if (self._flags.execution_mode == 'challenge-map' - or self._flags.execution_mode == 'challenge-sensors'): - self._pose_stream.send(pose_msg) - self._pose_stream.send(erdos.WatermarkMessage(timestamp)) + self._pose_stream.send(pose_msg) + self._pose_stream.send(erdos.WatermarkMessage(timestamp)) else: raise NotImplementedError( "Need pose message to initialize the estimates.") From c8fa7b22511b20aade3650a9e150e9af6f9146ad Mon Sep 17 00:00:00 2001 From: Peter Schafhalter Date: Wed, 7 Jul 2021 09:45:10 -1000 Subject: [PATCH 15/47] Support arbitrary traffic manager ports (#202) --- pylot/simulation/carla_operator.py | 4 +++- pylot/simulation/utils.py | 21 +++++++++++++-------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/pylot/simulation/carla_operator.py b/pylot/simulation/carla_operator.py index 0bc4bf31..da59b7ea 100644 --- a/pylot/simulation/carla_operator.py +++ b/pylot/simulation/carla_operator.py @@ -110,7 +110,9 @@ def __init__( # Spawn ego vehicle, people and vehicles. (self._ego_vehicle, self._vehicle_ids, self._people) = pylot.simulation.utils.spawn_actors( - self._client, self._world, self._simulator_version, + self._client, self._world, + self._flags.carla_traffic_manager_port, + self._simulator_version, self._flags.simulator_spawn_point_index, self._flags.control == 'simulator_auto_pilot', self._flags.simulator_num_people, diff --git a/pylot/simulation/utils.py b/pylot/simulation/utils.py index 653d00c8..214aab1a 100644 --- a/pylot/simulation/utils.py +++ b/pylot/simulation/utils.py @@ -166,11 +166,13 @@ def reset_world(world): actor.destroy() -def spawn_actors(client, world, simulator_version: str, - ego_spawn_point_index: int, auto_pilot: bool, num_people: int, - num_vehicles: int, logger): - vehicle_ids = spawn_vehicles(client, world, num_vehicles, logger) - ego_vehicle = spawn_ego_vehicle(world, ego_spawn_point_index, auto_pilot) +def spawn_actors(client, world, traffic_manager_port: int, + simulator_version: str, ego_spawn_point_index: int, + auto_pilot: bool, num_people: int, num_vehicles: int, logger): + vehicle_ids = spawn_vehicles(client, world, traffic_manager_port, + num_vehicles, logger) + ego_vehicle = spawn_ego_vehicle(world, traffic_manager_port, + ego_spawn_point_index, auto_pilot) people = [] if check_simulator_version(simulator_version, @@ -189,6 +191,7 @@ def spawn_actors(client, world, simulator_version: str, def spawn_ego_vehicle(world, + traffic_manager_port: int, spawn_point_index: int, auto_pilot: bool, blueprint: str = 'vehicle.lincoln.mkz2017'): @@ -207,7 +210,7 @@ def spawn_ego_vehicle(world, ego_vehicle = world.try_spawn_actor(v_blueprint, start_pose) if auto_pilot: - ego_vehicle.set_autopilot(True) + ego_vehicle.set_autopilot(True, traffic_manager_port) return ego_vehicle @@ -271,7 +274,8 @@ def spawn_people(client, world, num_people: int, logger): return (ped_ids, ped_control_ids) -def spawn_vehicles(client, world, num_vehicles: int, logger): +def spawn_vehicles(client, world, traffic_manager_port: int, num_vehicles: int, + logger): """ Spawns vehicles at random locations inside the world. Args: @@ -309,7 +313,8 @@ def spawn_vehicles(client, world, num_vehicles: int, logger): batch.append( command.SpawnActor(blueprint, transform).then( - command.SetAutopilot(command.FutureActor, True))) + command.SetAutopilot(command.FutureActor, True, + traffic_manager_port))) # Apply the batch and retrieve the identifiers. vehicle_ids = [] From 1518c0a14ff1dc66a3c2cfd38862ca468084ec5c Mon Sep 17 00:00:00 2001 From: Ionel Gog Date: Fri, 9 Jul 2021 09:09:24 -0700 Subject: [PATCH 16/47] [Bug] Ensure that the obstacle and TL bbox loggers do not overwrite bbox files. (#205) --- data_gatherer.py | 6 ++++-- pylot/loggers/bounding_box_logger_operator.py | 11 +++++++---- pylot/operator_creator.py | 3 ++- pylot/simulation/perfect_lane_detector_operator.py | 4 +--- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/data_gatherer.py b/data_gatherer.py index 0291f360..cc8adacc 100644 --- a/data_gatherer.py +++ b/data_gatherer.py @@ -162,7 +162,8 @@ def main(argv): traffic_light_depth_camera_stream, traffic_light_segmented_camera_stream, pose_stream) - pylot.operator_creator.add_bounding_box_logging(traffic_lights_stream) + pylot.operator_creator.add_bounding_box_logging( + traffic_lights_stream, 'tl-bboxes') if FLAGS.log_left_right_cameras: (left_camera_stream, right_camera_stream, _, @@ -186,7 +187,8 @@ def main(argv): depth_camera_stream, center_camera_stream, segmented_stream, pose_stream, ground_obstacles_stream, ground_speed_limit_signs_stream, ground_stop_signs_stream) - pylot.operator_creator.add_bounding_box_logging(obstacles_stream) + pylot.operator_creator.add_bounding_box_logging( + obstacles_stream, 'bboxes') if FLAGS.log_multiple_object_tracker: pylot.operator_creator.add_multiple_object_tracker_logging( diff --git a/pylot/loggers/bounding_box_logger_operator.py b/pylot/loggers/bounding_box_logger_operator.py index 1618f9bd..e6225dcc 100644 --- a/pylot/loggers/bounding_box_logger_operator.py +++ b/pylot/loggers/bounding_box_logger_operator.py @@ -21,13 +21,15 @@ class BoundingBoxLoggerOperator(erdos.Operator): _data_path (:obj:`str`): Directory to which to log files. """ def __init__(self, obstacles_stream: erdos.ReadStream, - finished_indicator_stream: erdos.WriteStream, flags): + finished_indicator_stream: erdos.WriteStream, flags, + file_base_name): obstacles_stream.add_callback(self.on_obstacles_msg) self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) self._flags = flags + self._file_base_name = file_base_name self._msg_cnt = 0 - self._data_path = os.path.join(self._flags.data_path, 'bboxes') + self._data_path = os.path.join(self._flags.data_path, file_base_name) os.makedirs(self._data_path, exist_ok=True) @staticmethod @@ -57,7 +59,8 @@ def on_obstacles_msg(self, msg: erdos.Message): assert len(msg.timestamp.coordinates) == 1 timestamp = msg.timestamp.coordinates[0] # Write the bounding boxes. - file_name = os.path.join(self._data_path, - 'bboxes-{}.json'.format(timestamp)) + file_name = os.path.join( + self._data_path, '{}-{}.json'.format(self._file_base_name, + timestamp)) with open(file_name, 'w') as outfile: json.dump(bboxes, outfile) diff --git a/pylot/operator_creator.py b/pylot/operator_creator.py index c011c28a..53d7f425 100644 --- a/pylot/operator_creator.py +++ b/pylot/operator_creator.py @@ -662,6 +662,7 @@ def add_planning_pose_synchronizer(waypoint_stream, pose_stream, def add_bounding_box_logging(obstacles_stream, + file_base_name, name='bounding_box_logger_operator'): from pylot.loggers.bounding_box_logger_operator import \ BoundingBoxLoggerOperator @@ -671,7 +672,7 @@ def add_bounding_box_logging(obstacles_stream, profile_file_name=FLAGS.profile_file_name) [finished_indicator_stream] = erdos.connect(BoundingBoxLoggerOperator, op_config, [obstacles_stream], - FLAGS) + FLAGS, file_base_name) return finished_indicator_stream diff --git a/pylot/simulation/perfect_lane_detector_operator.py b/pylot/simulation/perfect_lane_detector_operator.py index e4b12103..81a9b5ba 100644 --- a/pylot/simulation/perfect_lane_detector_operator.py +++ b/pylot/simulation/perfect_lane_detector_operator.py @@ -36,8 +36,6 @@ def __init__(self, pose_stream: ReadStream, open_drive_stream: ReadStream, self._flags = flags self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) - self._logger_lane = erdos.utils.setup_logging( - self.config.name + "_lane", self.config.log_file_name + "_lane") self._bgr_msgs = deque() self._pose_msgs = deque() self._frame_cnt = 0 @@ -132,7 +130,7 @@ def on_position_update(self, timestamp: Timestamp, binary_img = Image.fromarray(binary_frame) instance_img.save(instance_file_name) binary_img.save(binary_file_name) - self._logger_lane.debug( + self._logger.debug( '@{}: Created binary lane and lane images in {}'. format(pose_msg.timestamp, self._flags.data_path)) else: From efa9333933d1ed419ce37bf4aaeb59fed377f7e1 Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 13 Jul 2021 21:38:15 -0400 Subject: [PATCH 17/47] removed pygame depedencies from operator_creator.py --- pylot/operator_creator.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/pylot/operator_creator.py b/pylot/operator_creator.py index 9b80bff2..87ef7ba2 100644 --- a/pylot/operator_creator.py +++ b/pylot/operator_creator.py @@ -801,12 +801,6 @@ def add_visualizer(pose_stream=None, control_stream=None, name='visualizer_operator'): from pylot.debug.visualizer_operator import VisualizerOperator - import pygame - pygame.init() - pygame_display = pygame.display.set_mode( - (FLAGS.camera_image_width, FLAGS.camera_image_height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - pygame.display.set_caption("Pylot") streams_to_send_top_on = [] if pose_stream is None: pose_stream = erdos.IngestStream() @@ -875,7 +869,7 @@ def add_visualizer(pose_stream=None, obstacles_stream, traffic_lights_stream, tracked_obstacles_stream, lane_detection_stream, prediction_stream, waypoints_stream, control_stream, control_display_stream - ], pygame_display, FLAGS) + ], FLAGS) return control_display_stream, streams_to_send_top_on From 88dc197ef15e584a983bfcf1a4f5b2d37624f1ee Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:10:24 -0400 Subject: [PATCH 18/47] minor comment syntax fix Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index a7f3f351..4e3c31cf 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -356,8 +356,8 @@ def on_watermark(self, timestamp): image_np = frame.as_rgb_numpy_array() self.pub["PlanningWorld"].publish(image_np) - # todo: render text in foxglove - #self.render_text(pose_msg.data, control_msg, timestamp) + # TODO: render text in foxglove + # self.render_text(pose_msg.data, control_msg, timestamp) def run(self): # Run method is invoked after all operators finished initializing. From 7227e585f498b3558f781c28e4b1735f46cb8692 Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:10:47 -0400 Subject: [PATCH 19/47] minor comment syntax fix Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 4e3c31cf..eedf0909 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -132,7 +132,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) - # PYLOT-ROS Integration + # Pylot-ROS Integration for Foxglove visualization rospy.init_node("visualizer", anonymous=True, disable_signals=True) self.pub = {} # dict of publishers From ba82d01704b2e1b22d6719e3746de5e0014f7b01 Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:11:10 -0400 Subject: [PATCH 20/47] changed import statements to include full path Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index eedf0909..603c20f0 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -17,8 +17,8 @@ DEFAULT_VIS_TIME = 30000.0 import rospy -from .ros_camera_publisher import ROSCameraPublisher -from .ros_lidar_publisher import ROSLIDARPublisher +from pylot.debug.ros_camera_publisher import ROSCameraPublisher +from pylot.debug.ros_lidar_publisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state From 051f1a88e8e59c37d607fe74a5a66447649ecadd Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 18 Jul 2021 23:31:25 -0400 Subject: [PATCH 21/47] added docstrings to ROSCameraPublisher and ROSLIDARPublisher classes and methods, and added comments to visualizer operator --- pylot/debug/ros_camera_publisher.py | 16 +++++++++++++--- pylot/debug/ros_lidar_publisher.py | 15 ++++++++++++++- pylot/debug/visualizer_operator.py | 4 +++- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/pylot/debug/ros_camera_publisher.py b/pylot/debug/ros_camera_publisher.py index 84a49256..75872dae 100644 --- a/pylot/debug/ros_camera_publisher.py +++ b/pylot/debug/ros_camera_publisher.py @@ -4,14 +4,24 @@ from sensor_msgs.msg import Image class ROSCameraPublisher: + """Class that stores a ROS publisher node that publishes ROS Image messages + + Args: + topic: the name of the topic published to + + Attributes: + image_pub: ROS publisher node + """ def __init__(self, topic:str): - # publishes to the given topic self.image_pub = rospy.Publisher(topic, Image, queue_size=10) def publish(self, img_arr): - # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype - # inspired by https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py + """Publishes a sensor_msgs/Image message (constructed from input) + + Args: + img_arr: A numpy array storing a frame (e.g. camera, depth, etc.) + """ img_msg = Image(encoding='rgb8') if type(img_arr[0][0][0]) != np.int8: img_arr = img_arr.astype(np.int8) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index ef68c126..d655fe1f 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -4,12 +4,25 @@ from sensor_msgs.msg import PointCloud2, PointField class ROSLIDARPublisher: + """Class that stores a ROS publisher node that publishes ROS Point Cloud messages + + Args: + topic: the name of the topic published to + + Attributes: + point_cloud_pub: ROS publisher node + """ def __init__(self, topic:str): - # publishes to the given topic self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) def publish(self, points): + """Publishes a sensor_msgs/PointCloud2 message (constructed from input) + + Args: + points: A numpy array storing a point cloud (see pylot.pylot.perception.point_cloud) + """ + # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) points_byte_array = points.tobytes() diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index a7f3f351..81f3c7ec 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -8,6 +8,7 @@ import erdos import numpy as np +import rospy import pylot.utils from pylot.drivers.sensor_setup import RGBCameraSetup @@ -16,7 +17,6 @@ DEFAULT_VIS_TIME = 30000.0 -import rospy from .ros_camera_publisher import ROSCameraPublisher from .ros_lidar_publisher import ROSLIDARPublisher @@ -309,6 +309,8 @@ def on_watermark(self, timestamp): obstacle_prediction.draw_trajectory_on_frame(frame) if self._flags.visualize_lidar and point_cloud_msg: points = point_cloud_msg.point_cloud.points + # need to switch and reverse axes (from Velodyne coordinate space) + # for correct visualization in foxglove points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] points[:,0] = -points[:,0] From ba218efca88944f0e0a70079dc9d5220cb7369c0 Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 18 Jul 2021 23:42:39 -0400 Subject: [PATCH 22/47] minor comment changes --- pylot/debug/ros_lidar_publisher.py | 1 - 1 file changed, 1 deletion(-) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index d655fe1f..5163e9ae 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -23,7 +23,6 @@ def publish(self, points): points: A numpy array storing a point cloud (see pylot.pylot.perception.point_cloud) """ - # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) points_byte_array = points.tobytes() row_step = len(points_byte_array) From fdba02482f7be134a106479c96ad43273f4235d3 Mon Sep 17 00:00:00 2001 From: Eric Leong Date: Sun, 18 Jul 2021 20:51:19 -0700 Subject: [PATCH 23/47] Added QDTrack object tracker (#183) --- install.sh | 14 ++++ pylot/component_creator.py | 5 ++ pylot/flags.py | 7 +- pylot/operator_creator.py | 13 ++++ pylot/perception/detection/lane.py | 1 - pylot/perception/flags.py | 9 +++ .../perception/tracking/qd_track_operator.py | 74 +++++++++++++++++++ requirements.txt | 2 + 8 files changed, 121 insertions(+), 4 deletions(-) create mode 100644 pylot/perception/tracking/qd_track_operator.py diff --git a/install.sh b/install.sh index fca7f4cc..632c0675 100755 --- a/install.sh +++ b/install.sh @@ -78,6 +78,11 @@ mkdir -p tracking/center_track ; cd tracking/center_track # COCO model ~/.local/bin/gdown https://drive.google.com/uc?id=1tJCEJmdtYIh8VuN8CClGNws3YO7QGd40 +###### Download QDTrack models ###### +cd $PYLOT_HOME/dependencies/models +mkdir -p tracking/qd_track ; cd tracking/qd_track +~/.local/bin/gdown https://drive.google.com/uc?id=1YNAQgd8rMqqEG-fRj3VWlO4G5kdwJbxz + ##### Download AnyNet depth estimation models ##### echo "[x] Downloading the depth estimation models..." cd $PYLOT_HOME/dependencies/models @@ -140,6 +145,15 @@ sudo apt-get install llvm-9 export LLVM_CONFIG=/usr/bin/llvm-config-9 python3 setup.py build develop --user +###### Install QDTrack ###### +cd $PYLOT_HOME/dependencies/ +git clone https://github.com/mageofboy/qdtrack.git +git clone https://github.com/open-mmlab/mmdetection.git +cd mmdetection +python3 setup.py develop #need to add mmcv +cd $PYLOT_HOME/dependencies/qdtrack +python3 setup.py develop + ##### Download the Lanenet code ##### echo "[x] Cloning the lanenet lane detection code..." cd $PYLOT_HOME/dependencies/ diff --git a/pylot/component_creator.py b/pylot/component_creator.py index 8df37629..d4bb474c 100644 --- a/pylot/component_creator.py +++ b/pylot/component_creator.py @@ -354,6 +354,11 @@ def add_obstacle_tracking(center_camera_stream, obstacles_wo_history_tracking_stream = \ pylot.operator_creator.add_center_track_tracking( center_camera_stream, center_camera_setup) + elif FLAGS.tracker_type == 'qd_track': + logger.debug('Using QDTrack obstacle tracker...') + obstacles_wo_history_tracking_stream = \ + pylot.operator_creator.add_qd_track_tracking( + center_camera_stream, center_camera_setup) else: logger.debug('Using obstacle tracker...') obstacles_wo_history_tracking_stream = \ diff --git a/pylot/flags.py b/pylot/flags.py index bbf48234..7b88c7f7 100644 --- a/pylot/flags.py +++ b/pylot/flags.py @@ -40,9 +40,10 @@ 'True to enable obstacle tracking operator') flags.DEFINE_bool('perfect_obstacle_tracking', False, 'True to enable perfect obstacle tracking') -flags.DEFINE_enum('tracker_type', 'sort', - ['da_siam_rpn', 'deep_sort', 'sort', 'center_track'], - 'Sets which obstacle tracker to use') +flags.DEFINE_enum( + 'tracker_type', 'sort', + ['da_siam_rpn', 'deep_sort', 'sort', 'center_track', 'qd_track'], + 'Sets which obstacle tracker to use') flags.DEFINE_bool('lane_detection', False, 'True to enable lane detection') flags.DEFINE_bool('perfect_lane_detection', False, 'True to enable perfect lane detection') diff --git a/pylot/operator_creator.py b/pylot/operator_creator.py index 53d7f425..2274901e 100644 --- a/pylot/operator_creator.py +++ b/pylot/operator_creator.py @@ -269,6 +269,19 @@ def add_center_track_tracking(bgr_camera_stream, return obstacle_tracking_stream +def add_qd_track_tracking(bgr_camera_stream, camera_setup, name='qd_track'): + from pylot.perception.tracking.qd_track_operator import \ + QdTrackOperator + op_config = erdos.OperatorConfig(name='qd_track_operator', + log_file_name=FLAGS.log_file_name, + csv_log_file_name=FLAGS.csv_log_file_name, + profile_file_name=FLAGS.profile_file_name) + [obstacle_tracking_stream] = erdos.connect(QdTrackOperator, op_config, + [bgr_camera_stream], FLAGS, + camera_setup) + return obstacle_tracking_stream + + def add_tracking_evaluation(obstacle_tracking_stream, ground_obstacles_stream, evaluate_timely=False, diff --git a/pylot/perception/detection/lane.py b/pylot/perception/detection/lane.py index 3cc3e230..70069d83 100644 --- a/pylot/perception/detection/lane.py +++ b/pylot/perception/detection/lane.py @@ -5,7 +5,6 @@ import numpy as np from pylot.utils import Location, Rotation, Transform, Vector3D - from shapely.geometry import Point from shapely.geometry.polygon import Polygon diff --git a/pylot/perception/flags.py b/pylot/perception/flags.py index 4ed85692..a72b94a8 100644 --- a/pylot/perception/flags.py +++ b/pylot/perception/flags.py @@ -84,6 +84,15 @@ ['kitti_tracking', 'coco', 'mot', 'nuscenes'], 'CenterTrack available models') +# QDTrack tracking flags. +flags.DEFINE_string( + 'qd_track_model_path', 'dependencies/models/tracking/qd_track/' + + 'qdtrack-frcnn_r50_fpn_12e_bdd100k-13328aed.pth', 'Path to the model') +flags.DEFINE_string( + 'qd_track_config_path', + 'dependencies/qdtrack/configs/qdtrack-frcnn_r50_fpn_12e_bdd100k.py', + 'Path to the model') + # Lane detection flags. flags.DEFINE_float('lane_detection_gpu_memory_fraction', 0.3, 'GPU memory fraction allocated to Lanenet') diff --git a/pylot/perception/tracking/qd_track_operator.py b/pylot/perception/tracking/qd_track_operator.py new file mode 100644 index 00000000..2439c78d --- /dev/null +++ b/pylot/perception/tracking/qd_track_operator.py @@ -0,0 +1,74 @@ +import time + +import erdos + +from pylot.perception.detection.obstacle import Obstacle +from pylot.perception.detection.utils import BoundingBox2D, \ + OBSTACLE_LABELS +from pylot.perception.messages import ObstaclesMessage + + +class QdTrackOperator(erdos.Operator): + def __init__(self, camera_stream, obstacle_tracking_stream, flags, + camera_setup): + from qdtrack.apis import init_model + + camera_stream.add_callback(self.on_frame_msg, + [obstacle_tracking_stream]) + self._flags = flags + self._logger = erdos.utils.setup_logging(self.config.name, + self.config.log_file_name) + self._csv_logger = erdos.utils.setup_csv_logging( + self.config.name + '-csv', self.config.csv_log_file_name) + self._camera_setup = camera_setup + self.model = init_model(self._flags.qd_track_config_path, + checkpoint=self._flags.qd_track_model_path, + device='cuda:0', + cfg_options=None) + self.classes = ('pedestrian', 'rider', 'car', 'bus', 'truck', + 'bicycle', 'motorcycle', 'train') + self.frame_id = 0 + + @staticmethod + def connect(camera_stream): + obstacle_tracking_stream = erdos.WriteStream() + return [obstacle_tracking_stream] + + def destroy(self): + self._logger.warn('destroying {}'.format(self.config.name)) + + @erdos.profile_method() + def on_frame_msg(self, msg, obstacle_tracking_stream): + """Invoked when a FrameMessage is received on the camera stream.""" + from qdtrack.apis import inference_model + + self._logger.debug('@{}: {} received frame'.format( + msg.timestamp, self.config.name)) + assert msg.frame.encoding == 'BGR', 'Expects BGR frames' + start_time = time.time() + image_np = msg.frame.as_bgr_numpy_array() + results = inference_model(self.model, image_np, self.frame_id) + self.frame_id += 1 + + bbox_result, track_result = results.values() + obstacles = [] + for k, v in track_result.items(): + track_id = k + bbox = v['bbox'][None, :] + score = bbox[4] + label_id = v['label'] + label = self.classes[label_id] + if label in ['pedestrian', 'rider']: + label = 'person' + if label in OBSTACLE_LABELS: + bounding_box_2D = BoundingBox2D(bbox[0], bbox[2], bbox[1], + bbox[3]) + obstacles.append( + Obstacle(bounding_box_2D, + score, + label, + track_id, + bounding_box_2D=bounding_box_2D)) + runtime = (time.time() - start_time) * 1000 + obstacle_tracking_stream.send( + ObstaclesMessage(msg.timestamp, obstacles, runtime)) diff --git a/requirements.txt b/requirements.txt index 5db6b07f..c300a918 100644 --- a/requirements.txt +++ b/requirements.txt @@ -27,5 +27,7 @@ nuscenes-devkit progress pyquaternion scikit-learn==0.22.2 +mmcv>=0.3.0 +mmdet ##### CARLA dependencies ##### networkx==2.2 From a24ed7069ec4ddfeaa5e7d2c6c8216e91075ca22 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 19 Jul 2021 02:21:45 -0400 Subject: [PATCH 24/47] yapf'd deez files --- pylot/debug/ros_camera_publisher.py | 15 +++++++-------- pylot/debug/ros_lidar_publisher.py | 16 ++++++++++------ pylot/debug/visualizer_operator.py | 28 ++++++++++++++++------------ 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/pylot/debug/ros_camera_publisher.py b/pylot/debug/ros_camera_publisher.py index 75872dae..83c90ada 100644 --- a/pylot/debug/ros_camera_publisher.py +++ b/pylot/debug/ros_camera_publisher.py @@ -3,6 +3,7 @@ import numpy as np from sensor_msgs.msg import Image + class ROSCameraPublisher: """Class that stores a ROS publisher node that publishes ROS Image messages @@ -12,10 +13,9 @@ class ROSCameraPublisher: Attributes: image_pub: ROS publisher node """ - - def __init__(self, topic:str): + def __init__(self, topic: str): self.image_pub = rospy.Publisher(topic, Image, queue_size=10) - + def publish(self, img_arr): """Publishes a sensor_msgs/Image message (constructed from input) @@ -27,9 +27,8 @@ def publish(self, img_arr): img_arr = img_arr.astype(np.int8) img_msg.height, img_msg.width, channels = img_arr.shape img_msg.data = img_arr.tobytes() - img_msg.step = img_msg.width * img_msg.height - img_msg.is_bigendian = ( - img_arr.dtype.byteorder == '>' or - img_arr.dtype.byteorder == '=' and sys.byteorder == 'big' - ) + img_msg.step = img_msg.width * img_msg.height + img_msg.is_bigendian = (img_arr.dtype.byteorder == '>' + or img_arr.dtype.byteorder == '=' + and sys.byteorder == 'big') self.image_pub.publish(img_msg) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index 5163e9ae..37449887 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -3,6 +3,7 @@ import numpy as np from sensor_msgs.msg import PointCloud2, PointField + class ROSLIDARPublisher: """Class that stores a ROS publisher node that publishes ROS Point Cloud messages @@ -12,10 +13,11 @@ class ROSLIDARPublisher: Attributes: point_cloud_pub: ROS publisher node """ + def __init__(self, topic: str): + self.point_cloud_pub = rospy.Publisher(topic, + PointCloud2, + queue_size=10) - def __init__(self, topic:str): - self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) - def publish(self, points): """Publishes a sensor_msgs/PointCloud2 message (constructed from input) @@ -27,9 +29,11 @@ def publish(self, points): points_byte_array = points.tobytes() row_step = len(points_byte_array) point_step = len(points[0].tobytes()) - fields = [PointField('x', 0, PointField.FLOAT32, 1), - PointField('y', 4, PointField.FLOAT32, 1), - PointField('z', 8, PointField.FLOAT32, 1)] + fields = [ + PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1) + ] point_cloud_msg = PointCloud2(height=1, width=len(points), is_dense=True, diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index d9b15b65..b7d4d07e 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -134,7 +134,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, # Pylot-ROS Integration for Foxglove visualization rospy.init_node("visualizer", anonymous=True, disable_signals=True) - self.pub = {} # dict of publishers + self.pub = {} # dict of publishers # Creating ROS publishers for streams to be visualized if flags.visualize_rgb_camera: @@ -142,7 +142,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_detected_obstacles: self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: - self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") + self.pub["TrackedObstacle"] = ROSCameraPublisher( + "/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: @@ -155,7 +156,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self.pub["Depth"] = ROSCameraPublisher("/camera/depth") self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: - self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") + self.pub["Segmentation"] = ROSCameraPublisher( + "/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) top_down_transform = pylot.utils.get_top_down_transform( @@ -165,7 +167,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._bird_eye_camera_setup = RGBCameraSetup( 'bird_eye_camera', flags.camera_image_width, flags.camera_image_height, top_down_transform, 90) - self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") + self.pub["PlanningWorld"] = ROSCameraPublisher( + "/camera/planning_world") else: self._planning_world = None @@ -311,10 +314,10 @@ def on_watermark(self, timestamp): points = point_cloud_msg.point_cloud.points # need to switch and reverse axes (from Velodyne coordinate space) # for correct visualization in foxglove - points[:,[0,2]] = points[:,[2,0]] - points[:,[1,2]] = points[:,[2,1]] - points[:,0] = -points[:,0] - points[:,2] = -points[:,2] + points[:, [0, 2]] = points[:, [2, 0]] + points[:, [1, 2]] = points[:, [2, 1]] + points[:, 0] = -points[:, 0] + points[:, 2] = -points[:, 2] self.pub["PointCloud"].publish(points) if self._flags.visualize_detected_lanes and bgr_msg and lane_detection_msg: for lane in lane_detection_msg.data: @@ -327,17 +330,18 @@ def on_watermark(self, timestamp): self.pub["Depth"].publish(image_np) depth_msg.frame.resize(854, 480) points = depth_msg.frame.as_point_cloud() - points[:,0] = -points[:,0] + points[:, 0] = -points[:, 0] self.pub["DepthPointCloud"].publish(points) if self._flags.visualize_segmentation and segmentation_msg: - self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) + self.pub["Segmentation"].publish( + segmentation_msg.frame.as_cityscapes_palette()) if self._flags.visualize_world: if prediction_camera_msg is None: # Top-down prediction is not available. Show planning # world on a black image. black_img = np.zeros((self._bird_eye_camera_setup.height, - self._bird_eye_camera_setup.width, 3), - dtype=np.dtype("uint8")) + self._bird_eye_camera_setup.width, 3), + dtype=np.dtype("uint8")) frame = CameraFrame(black_img, 'RGB', self._bird_eye_camera_setup) else: From 13b6356ad9bb0670bc538b477be6b2662c268072 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 14 Jun 2021 20:26:06 -0400 Subject: [PATCH 25/47] added ROSCameraPublisher.py and visualizer operator can publish camera frame data to foxglove-studio --- pylot/debug/ROSCameraPublisher.py | 23 ++++++++++++++++ ...zer_operator.py => visualizer_operater.py} | 27 +++++++++++++++++-- 2 files changed, 48 insertions(+), 2 deletions(-) create mode 100644 pylot/debug/ROSCameraPublisher.py rename pylot/debug/{visualizer_operator.py => visualizer_operater.py} (93%) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py new file mode 100644 index 00000000..fbc41256 --- /dev/null +++ b/pylot/debug/ROSCameraPublisher.py @@ -0,0 +1,23 @@ +import sys +import rospy +import numpy as np +from sensor_msgs.msg import Image + +class ROSCameraPublisher: + + def __init__(self, topic:str): + # publishes to the given topic + self.image_pub = rospy.Publisher(topic, Image, queue_size=10) + + def publish(self, img_arr): + # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype + img_msg = Image(encoding='rgb8') + img_msg.height, img_msg.weight, channels = img_arr.shape + contig_arr = np.ascontiguousarray(img_arr) + img_msg.data = contig_arr.tobytes() + img_msg.step = img_msg.width * img_msg.height + img_msg.is_bigendian = ( + img_arr.dtype.byteorder == '>' or + img_arr.dtype.byteorder == '=' and sys.byteorder == 'big' + ) + self.image_pub.publish(img_msg) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operater.py similarity index 93% rename from pylot/debug/visualizer_operator.py rename to pylot/debug/visualizer_operater.py index ce069bf3..d83bdcba 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operater.py @@ -19,6 +19,8 @@ DEFAULT_VIS_TIME = 30000.0 +import rospy +from .ROSCameraPublisher import ROSCameraPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state @@ -141,6 +143,10 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, mono = pygame.font.match_font(mono) self.font = pygame.font.Font(mono, 14) + # PYLOT-ROS Integration + rospy.init_node("visualizer", anonymous=True, disable_signals=True) + self.pub = {} # dict of publishers + # Array of keys to figure out which message to display. self.current_display = 0 self.display_array = [] @@ -148,27 +154,35 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_rgb_camera: self.display_array.append("RGB") self.window_titles.append("RGB Camera") + self.pub["RBG"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: self.display_array.append("Obstacle") self.window_titles.append("Detected obstacles") + self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: self.display_array.append("TrackedObstacle") self.window_titles.append("Obstacle tracking") + self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: self.display_array.append("TLCamera") self.window_titles.append("Detected traffic lights") + self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: self.display_array.append("Waypoint") self.window_titles.append("Planning") + self.pub["Waypoint"] = ROSCameraPublisher("/camera/waypoint") if flags.visualize_prediction: self.display_array.append("PredictionCamera") self.window_titles.append("Prediction") + # self.pub["PredictionCamera"] = ROSCameraPublisher("/camera/prediction_camera") if flags.visualize_lidar: self.display_array.append("PointCloud") self.window_titles.append("LiDAR") + # self.pub["Lanes"] = ROSPointCloudPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: self.display_array.append("Lanes") self.window_titles.append("Detected lanes") + self.pub["Lanes"] = ROSCameraPublisher("/camera/lanes") if flags.visualize_depth_camera: self.display_array.append("Depth") self.window_titles.append("Depth Camera") @@ -318,22 +332,25 @@ def on_watermark(self, timestamp): sensor_to_display = self.display_array[self.current_display] if sensor_to_display == "RGB" and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif (sensor_to_display == "TLCamera" and tl_camera_msg and traffic_light_msg): tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, tl_camera_msg.frame) elif (sensor_to_display == "TrackedObstacle" and bgr_msg and tracked_obstacle_msg): bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) - bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg and waypoint_msg): bgr_frame = bgr_msg.frame @@ -344,6 +361,7 @@ def on_watermark(self, timestamp): if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) bgr_frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_frame) elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg and prediction_msg): frame = prediction_camera_msg.frame @@ -357,7 +375,7 @@ def on_watermark(self, timestamp): elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) - bgr_msg.frame.visualize(self.display, timestamp=timestamp) + self._publish_camera_frame(sensor_to_display, bgr_msg.frame) elif sensor_to_display == "Depth" and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) elif sensor_to_display == "Segmentation" and segmentation_msg: @@ -430,3 +448,8 @@ def _visualize_imu(self, msg): end_acc.as_simulator_location(), arrow_size=0.1, life_time=0.1) + + def _publish_camera_frame(self, sensor_to_display, frame): + image_np = frame.as_rgb_numpy_array() + self.pub[sensor_to_display].publish(image_np) + From 204d60c0f72fab27ef4413b4f6bcf641c68274a1 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 14 Jun 2021 20:27:47 -0400 Subject: [PATCH 26/47] minimal name changes --- pylot/debug/{visualizer_operater.py => visualizer_operator.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pylot/debug/{visualizer_operater.py => visualizer_operator.py} (100%) diff --git a/pylot/debug/visualizer_operater.py b/pylot/debug/visualizer_operator.py similarity index 100% rename from pylot/debug/visualizer_operater.py rename to pylot/debug/visualizer_operator.py From 5f77390fc32ac55a461de0f55ef31fd60a7a6051 Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 15 Jun 2021 03:43:51 -0400 Subject: [PATCH 27/47] fixed variable typo --- pylot/debug/ROSCameraPublisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index fbc41256..9c02553a 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -12,7 +12,7 @@ def __init__(self, topic:str): def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype img_msg = Image(encoding='rgb8') - img_msg.height, img_msg.weight, channels = img_arr.shape + img_msg.height, img_msg.width, channels = img_arr.shape contig_arr = np.ascontiguousarray(img_arr) img_msg.data = contig_arr.tobytes() img_msg.step = img_msg.width * img_msg.height From 0b50b995534011433e37a5826d25c92d1e396c71 Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 17 Jun 2021 01:07:38 -0400 Subject: [PATCH 28/47] visualizer operator can now publish ROS PointCloud2 messages (displayed in foxglove-studio) --- pylot/debug/ROSCameraPublisher.py | 3 +-- pylot/debug/ROSLIDARPublisher.py | 32 ++++++++++++++++++++++++++++++ pylot/debug/visualizer_operator.py | 7 ++++--- 3 files changed, 37 insertions(+), 5 deletions(-) create mode 100644 pylot/debug/ROSLIDARPublisher.py diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index 9c02553a..bff1d55a 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -13,8 +13,7 @@ def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype img_msg = Image(encoding='rgb8') img_msg.height, img_msg.width, channels = img_arr.shape - contig_arr = np.ascontiguousarray(img_arr) - img_msg.data = contig_arr.tobytes() + img_msg.data = img_arr.tobytes() img_msg.step = img_msg.width * img_msg.height img_msg.is_bigendian = ( img_arr.dtype.byteorder == '>' or diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py new file mode 100644 index 00000000..9200dbc2 --- /dev/null +++ b/pylot/debug/ROSLIDARPublisher.py @@ -0,0 +1,32 @@ +import sys +import rospy +import numpy as np +from sensor_msgs.msg import PointCloud2, PointField + +class ROSLIDARPublisher: + + def __init__(self, topic:str): + # publishes to the given topic + self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) + + def publish(self, points): + # converts point cloud points array to a sensor_msgs/PointCloud2 datatype + points = points.astype(np.float32) + points[:,[0,2]] = points[:,[2,0]] + points[:,[1,2]] = points[:,[2,1]] + points[:,2] = -points[:,2] + points_byte_array = points.tobytes() + row_step = len(points_byte_array) + point_step = len(points[0].tobytes()) + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + point_cloud_msg = PointCloud2(height=1, + width=len(points), + is_dense=True, + is_bigendian=False, + fields=fields, + point_step=point_step, + row_step=row_step, + data=points_byte_array) + self.point_cloud_pub.publish(point_cloud_msg) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index d83bdcba..471a3bc2 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -19,8 +19,10 @@ DEFAULT_VIS_TIME = 30000.0 +# PYLOT-ROS Integration import rospy from .ROSCameraPublisher import ROSCameraPublisher +from .ROSLIDARPublisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state @@ -178,7 +180,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_lidar: self.display_array.append("PointCloud") self.window_titles.append("LiDAR") - # self.pub["Lanes"] = ROSPointCloudPublisher("/lidar/point_cloud") + self.pub["PointCloud"] = ROSLIDARPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: self.display_array.append("Lanes") self.window_titles.append("Detected lanes") @@ -370,8 +372,7 @@ def on_watermark(self, timestamp): obstacle_prediction.draw_trajectory_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) elif sensor_to_display == "PointCloud" and point_cloud_msg: - point_cloud_msg.point_cloud.visualize(self.display, - timestamp=timestamp) + self.pub["PointCloud"].publish(point_cloud_msg.point_cloud.points) elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) From 10288a5df5ad4c573aab5976d741c54673f49e77 Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 17 Jun 2021 16:07:14 -0400 Subject: [PATCH 29/47] fixed mirror image for lidar --- pylot/debug/ROSLIDARPublisher.py | 1 + 1 file changed, 1 insertion(+) diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py index 9200dbc2..97bcf0d1 100644 --- a/pylot/debug/ROSLIDARPublisher.py +++ b/pylot/debug/ROSLIDARPublisher.py @@ -14,6 +14,7 @@ def publish(self, points): points = points.astype(np.float32) points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] + points[:,0] = -points[:,0] points[:,2] = -points[:,2] points_byte_array = points.tobytes() row_step = len(points_byte_array) From c3d091350267f620c0e99681fb5c21cd012c6cd2 Mon Sep 17 00:00:00 2001 From: Josh Date: Wed, 23 Jun 2021 04:23:21 -0400 Subject: [PATCH 30/47] visualizer operator can visualize segmentation frames and depth frames (both as rgb frame and point cloud), also cleaned up some code --- pylot/debug/ROSCameraPublisher.py | 3 ++ pylot/debug/ROSLIDARPublisher.py | 4 --- pylot/debug/visualizer_operator.py | 45 ++++++++++++++++++++---------- 3 files changed, 34 insertions(+), 18 deletions(-) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ROSCameraPublisher.py index bff1d55a..84a49256 100644 --- a/pylot/debug/ROSCameraPublisher.py +++ b/pylot/debug/ROSCameraPublisher.py @@ -11,7 +11,10 @@ def __init__(self, topic:str): def publish(self, img_arr): # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype + # inspired by https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py img_msg = Image(encoding='rgb8') + if type(img_arr[0][0][0]) != np.int8: + img_arr = img_arr.astype(np.int8) img_msg.height, img_msg.width, channels = img_arr.shape img_msg.data = img_arr.tobytes() img_msg.step = img_msg.width * img_msg.height diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ROSLIDARPublisher.py index 97bcf0d1..ef68c126 100644 --- a/pylot/debug/ROSLIDARPublisher.py +++ b/pylot/debug/ROSLIDARPublisher.py @@ -12,10 +12,6 @@ def __init__(self, topic:str): def publish(self, points): # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) - points[:,[0,2]] = points[:,[2,0]] - points[:,[1,2]] = points[:,[2,1]] - points[:,0] = -points[:,0] - points[:,2] = -points[:,2] points_byte_array = points.tobytes() row_step = len(points_byte_array) point_step = len(points[0].tobytes()) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 471a3bc2..896cd34e 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -19,7 +19,6 @@ DEFAULT_VIS_TIME = 30000.0 -# PYLOT-ROS Integration import rospy from .ROSCameraPublisher import ROSCameraPublisher from .ROSLIDARPublisher import ROSLIDARPublisher @@ -156,7 +155,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_rgb_camera: self.display_array.append("RGB") self.window_titles.append("RGB Camera") - self.pub["RBG"] = ROSCameraPublisher("/camera/rgb") + self.pub["RGB"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: self.display_array.append("Obstacle") self.window_titles.append("Detected obstacles") @@ -188,9 +187,13 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_depth_camera: self.display_array.append("Depth") self.window_titles.append("Depth Camera") + #self.pub["Depth"] = ROSLIDARPublisher("/lidar/depth") # need to create a flag called --visualize_depth_point_cloud + self.pub["Depth"] = ROSCameraPublisher("/camera/depth") + self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: self.display_array.append("Segmentation") self.window_titles.append("Segmentation") + self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) top_down_transform = pylot.utils.get_top_down_transform( @@ -334,25 +337,29 @@ def on_watermark(self, timestamp): sensor_to_display = self.display_array[self.current_display] if sensor_to_display == "RGB" and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["RGB"].publish(image_np) elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["Obstacle"].publish(image_np) elif (sensor_to_display == "TLCamera" and tl_camera_msg and traffic_light_msg): tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, tl_camera_msg.frame) + image_np = tl_camera_msg.frame.as_rgb_numpy_array() + self.pub["TLCamera"].publish(image_np) elif (sensor_to_display == "TrackedObstacle" and bgr_msg and tracked_obstacle_msg): bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["TrackedObstacle"].publish(image_np) elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg and waypoint_msg): bgr_frame = bgr_msg.frame @@ -363,7 +370,8 @@ def on_watermark(self, timestamp): if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) bgr_frame.visualize(self.display, timestamp=timestamp) - self._publish_camera_frame(sensor_to_display, bgr_frame) + image_np = bgr_frame.as_rgb_numpy_array() + self.pub["Waypoint"].publish(image_np) elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg and prediction_msg): frame = prediction_camera_msg.frame @@ -372,15 +380,29 @@ def on_watermark(self, timestamp): obstacle_prediction.draw_trajectory_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) elif sensor_to_display == "PointCloud" and point_cloud_msg: - self.pub["PointCloud"].publish(point_cloud_msg.point_cloud.points) + points = point_cloud_msg.point_cloud.points + points[:,[0,2]] = points[:,[2,0]] + points[:,[1,2]] = points[:,[2,1]] + points[:,0] = -points[:,0] + points[:,2] = -points[:,2] + self.pub["PointCloud"].publish(points) elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) - self._publish_camera_frame(sensor_to_display, bgr_msg.frame) + image_np = bgr_msg.frame.as_rgb_numpy_array() + self.pub["Lanes"].publish(image_np) elif sensor_to_display == "Depth" and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) + image_np = depth_msg.frame.original_frame + image_np = image_np[:, :, ::-1] + self.pub["Depth"].publish(image_np) + depth_msg.frame.resize(854, 480) + points = depth_msg.frame.as_point_cloud() + points[:,0] = -points[:,0] + self.pub["DepthPointCloud"].publish(points) elif sensor_to_display == "Segmentation" and segmentation_msg: segmentation_msg.frame.visualize(self.display, timestamp=timestamp) + self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) elif sensor_to_display == "PlanningWorld": if prediction_camera_msg is None: # Top-down prediction is not available. Show planning @@ -449,8 +471,3 @@ def _visualize_imu(self, msg): end_acc.as_simulator_location(), arrow_size=0.1, life_time=0.1) - - def _publish_camera_frame(self, sensor_to_display, frame): - image_np = frame.as_rgb_numpy_array() - self.pub[sensor_to_display].publish(image_np) - From 630b943add37b571425330ea1a1206e3a9559a6f Mon Sep 17 00:00:00 2001 From: Josh Date: Wed, 23 Jun 2021 22:04:38 -0400 Subject: [PATCH 31/47] visualizes planning world --- pylot/debug/visualizer_operator.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 896cd34e..cf355f0b 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -205,6 +205,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, flags.camera_image_height, top_down_transform, 90) self.display_array.append("PlanningWorld") self.window_titles.append("Planning world") + self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") else: self._planning_world = None assert len(self.display_array) == len(self.window_titles), \ @@ -428,6 +429,8 @@ def on_watermark(self, timestamp): self._planning_world.update_waypoints(None, waypoint_msg.waypoints) self._planning_world.draw_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) + image_np = frame.as_rgb_numpy_array() + self.pub["PlanningWorld"].publish(image_np) self.render_text(pose_msg.data, control_msg, timestamp) From b3b0aabeddbe6fbcf96754ece3606a91aad96d2c Mon Sep 17 00:00:00 2001 From: Josh Date: Sat, 3 Jul 2021 19:48:01 -0400 Subject: [PATCH 32/47] streams can be displayed simultaneously (in foxglove), still has pygame code to be removed --- pylot/debug/visualizer_operator.py | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index cf355f0b..7a07a8ab 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -336,33 +336,30 @@ def on_watermark(self, timestamp): self._visualize_imu(imu_msg) sensor_to_display = self.display_array[self.current_display] - if sensor_to_display == "RGB" and bgr_msg: + if self._flags.visualize_rgb_camera and bgr_msg: bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["RGB"].publish(image_np) - elif sensor_to_display == "Obstacle" and bgr_msg and obstacle_msg: + if self._flags.visualize_detected_obstacles and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Obstacle"].publish(image_np) - elif (sensor_to_display == "TLCamera" and tl_camera_msg - and traffic_light_msg): + if self._flags.visualize_detected_traffic_lights and tl_camera_msg and traffic_light_msg: tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) image_np = tl_camera_msg.frame.as_rgb_numpy_array() self.pub["TLCamera"].publish(image_np) - elif (sensor_to_display == "TrackedObstacle" and bgr_msg - and tracked_obstacle_msg): + if self._flags.visualize_tracked_obstacles and bgr_msg and tracked_obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes( timestamp, tracked_obstacle_msg.obstacle_trajectories, ego_transform) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["TrackedObstacle"].publish(image_np) - elif sensor_to_display == "Waypoint" and (bgr_msg and pose_msg - and waypoint_msg): + if self._flags.visualize_waypoints and bgr_msg and pose_msg and waypoint_msg: bgr_frame = bgr_msg.frame if self._flags.draw_waypoints_on_camera_frames: bgr_frame.camera_setup.set_transform( @@ -373,26 +370,25 @@ def on_watermark(self, timestamp): bgr_frame.visualize(self.display, timestamp=timestamp) image_np = bgr_frame.as_rgb_numpy_array() self.pub["Waypoint"].publish(image_np) - elif (sensor_to_display == "PredictionCamera" and prediction_camera_msg - and prediction_msg): + if prediction_camera_msg and prediction_msg: frame = prediction_camera_msg.frame frame.transform_to_cityscapes() for obstacle_prediction in prediction_msg.predictions: obstacle_prediction.draw_trajectory_on_frame(frame) frame.visualize(self.display, timestamp=timestamp) - elif sensor_to_display == "PointCloud" and point_cloud_msg: + if self._flags.visualize_lidar and point_cloud_msg: points = point_cloud_msg.point_cloud.points points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] points[:,0] = -points[:,0] points[:,2] = -points[:,2] self.pub["PointCloud"].publish(points) - elif (sensor_to_display == "Lanes" and bgr_msg and lane_detection_msg): + if self._flags.visualize_detected_lanes and bgr_msg and lane_detection_msg: for lane in lane_detection_msg.data: lane.draw_on_frame(bgr_msg.frame) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Lanes"].publish(image_np) - elif sensor_to_display == "Depth" and depth_msg: + if self._flags.visualize_depth_camera and depth_msg: depth_msg.frame.visualize(self.display, timestamp=timestamp) image_np = depth_msg.frame.original_frame image_np = image_np[:, :, ::-1] @@ -401,16 +397,16 @@ def on_watermark(self, timestamp): points = depth_msg.frame.as_point_cloud() points[:,0] = -points[:,0] self.pub["DepthPointCloud"].publish(points) - elif sensor_to_display == "Segmentation" and segmentation_msg: + if self._flags.visualize_segmentation and segmentation_msg: segmentation_msg.frame.visualize(self.display, timestamp=timestamp) self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) - elif sensor_to_display == "PlanningWorld": + if self._flags.visualize_world: if prediction_camera_msg is None: # Top-down prediction is not available. Show planning # world on a black image. black_img = np.zeros((self._bird_eye_camera_setup.height, - self._bird_eye_camera_setup.width, 3), - dtype=np.dtype("uint8")) + self._bird_eye_camera_setup.width, 3), + dtype=np.dtype("uint8")) frame = CameraFrame(black_img, 'RGB', self._bird_eye_camera_setup) else: From 389092b40d2cc05a1898a2a6362a2566ebb0b10c Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 4 Jul 2021 02:43:15 -0400 Subject: [PATCH 33/47] removed all dependencies on pygame in visualizer_operator, though still some pygame code in other files --- pylot/debug/visualizer_operator.py | 85 +++--------------------------- 1 file changed, 7 insertions(+), 78 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 7a07a8ab..5e1cd4b2 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -9,9 +9,6 @@ import numpy as np -import pygame -from pygame.locals import K_n - import pylot.utils from pylot.drivers.sensor_setup import RGBCameraSetup from pylot.perception.camera_frame import CameraFrame @@ -25,10 +22,10 @@ class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state - of the entire pipeline by visualizing it on a pygame instance. + of the entire pipeline by visualizing it in Foxglove Studio. This receives input data from almost the entire pipeline and renders the - results of the operator currently chosen by the developer on the screen. + results of the operator(s) in Foxglove Studio. """ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, prediction_camera_stream, depth_camera_stream, @@ -36,7 +33,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, obstacles_stream, traffic_lights_stream, tracked_obstacles_stream, lane_detection_stream, prediction_stream, waypoints_stream, control_stream, - display_control_stream, pygame_display, flags): + control_display_stream, flags): visualize_streams = [] self._pose_msgs = deque() pose_stream.add_callback( @@ -132,67 +129,32 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, erdos.add_watermark_callback(visualize_streams, [], self.on_watermark) # Add a callback on a control stream to figure out what to display. - display_control_stream.add_callback(self.change_display) self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) - self.display = pygame_display - - # Set the font. - fonts = [x for x in pygame.font.get_fonts() if 'mono' in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self.font = pygame.font.Font(mono, 14) # PYLOT-ROS Integration rospy.init_node("visualizer", anonymous=True, disable_signals=True) self.pub = {} # dict of publishers - # Array of keys to figure out which message to display. - self.current_display = 0 - self.display_array = [] - self.window_titles = [] + # Creating ROS publishers for streams to be visualized if flags.visualize_rgb_camera: - self.display_array.append("RGB") - self.window_titles.append("RGB Camera") self.pub["RGB"] = ROSCameraPublisher("/camera/rgb") if flags.visualize_detected_obstacles: - self.display_array.append("Obstacle") - self.window_titles.append("Detected obstacles") self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: - self.display_array.append("TrackedObstacle") - self.window_titles.append("Obstacle tracking") self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: - self.display_array.append("TLCamera") - self.window_titles.append("Detected traffic lights") self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: - self.display_array.append("Waypoint") - self.window_titles.append("Planning") self.pub["Waypoint"] = ROSCameraPublisher("/camera/waypoint") - if flags.visualize_prediction: - self.display_array.append("PredictionCamera") - self.window_titles.append("Prediction") - # self.pub["PredictionCamera"] = ROSCameraPublisher("/camera/prediction_camera") if flags.visualize_lidar: - self.display_array.append("PointCloud") - self.window_titles.append("LiDAR") self.pub["PointCloud"] = ROSLIDARPublisher("/lidar/point_cloud") if flags.visualize_detected_lanes: - self.display_array.append("Lanes") - self.window_titles.append("Detected lanes") self.pub["Lanes"] = ROSCameraPublisher("/camera/lanes") if flags.visualize_depth_camera: - self.display_array.append("Depth") - self.window_titles.append("Depth Camera") - #self.pub["Depth"] = ROSLIDARPublisher("/lidar/depth") # need to create a flag called --visualize_depth_point_cloud self.pub["Depth"] = ROSCameraPublisher("/camera/depth") self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: - self.display_array.append("Segmentation") - self.window_titles.append("Segmentation") self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) @@ -203,13 +165,9 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._bird_eye_camera_setup = RGBCameraSetup( 'bird_eye_camera', flags.camera_image_width, flags.camera_image_height, top_down_transform, 90) - self.display_array.append("PlanningWorld") - self.window_titles.append("Planning world") self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") else: self._planning_world = None - assert len(self.display_array) == len(self.window_titles), \ - "The display and titles differ." # Save the flags. self._flags = flags @@ -231,13 +189,6 @@ def save(self, msg, msg_type, queue): msg.timestamp, msg_type)) queue.append(msg) - def change_display(self, display_message): - if display_message.data == K_n: - self.current_display = (self.current_display + 1) % len( - self.display_array) - self._logger.debug("@{}: Visualizer changed to {}".format( - display_message.timestamp, self.current_display)) - def get_message(self, queue, timestamp, name): msg = None if queue: @@ -278,21 +229,7 @@ def render_text(self, pose, control, timestamp): "Reverse : {:.2f}".format(control.reverse), ] - # Display the information box. - info_surface = pygame.Surface( - (220, self._flags.camera_image_height // 3)) - info_surface.set_alpha(100) - self.display.blit(info_surface, (0, 0)) - - # Render the text. - v_offset = 10 - for line in info_text: - if v_offset + 18 > self._flags.camera_image_height: - break - surface = self.font.render(line, True, (255, 255, 255)) - self.display.blit(surface, (8, v_offset)) - v_offset += 18 - pygame.display.flip() + # TODO: Render text (using ROS publisher) def on_watermark(self, timestamp): self._logger.debug("@{}: received watermark.".format(timestamp)) @@ -335,22 +272,18 @@ def on_watermark(self, timestamp): if self._flags.visualize_imu: self._visualize_imu(imu_msg) - sensor_to_display = self.display_array[self.current_display] if self._flags.visualize_rgb_camera and bgr_msg: - bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["RGB"].publish(image_np) if self._flags.visualize_detected_obstacles and bgr_msg and obstacle_msg: bgr_msg.frame.annotate_with_bounding_boxes(timestamp, obstacle_msg.obstacles, ego_transform) - bgr_msg.frame.visualize(self.display, timestamp=timestamp) image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Obstacle"].publish(image_np) if self._flags.visualize_detected_traffic_lights and tl_camera_msg and traffic_light_msg: tl_camera_msg.frame.annotate_with_bounding_boxes( timestamp, traffic_light_msg.obstacles) - tl_camera_msg.frame.visualize(self.display, timestamp=timestamp) image_np = tl_camera_msg.frame.as_rgb_numpy_array() self.pub["TLCamera"].publish(image_np) if self._flags.visualize_tracked_obstacles and bgr_msg and tracked_obstacle_msg: @@ -367,7 +300,6 @@ def on_watermark(self, timestamp): waypoint_msg.waypoints.draw_on_frame(bgr_frame) if self._flags.draw_waypoints_on_world: waypoint_msg.waypoints.draw_on_world(self._world) - bgr_frame.visualize(self.display, timestamp=timestamp) image_np = bgr_frame.as_rgb_numpy_array() self.pub["Waypoint"].publish(image_np) if prediction_camera_msg and prediction_msg: @@ -375,7 +307,6 @@ def on_watermark(self, timestamp): frame.transform_to_cityscapes() for obstacle_prediction in prediction_msg.predictions: obstacle_prediction.draw_trajectory_on_frame(frame) - frame.visualize(self.display, timestamp=timestamp) if self._flags.visualize_lidar and point_cloud_msg: points = point_cloud_msg.point_cloud.points points[:,[0,2]] = points[:,[2,0]] @@ -389,7 +320,6 @@ def on_watermark(self, timestamp): image_np = bgr_msg.frame.as_rgb_numpy_array() self.pub["Lanes"].publish(image_np) if self._flags.visualize_depth_camera and depth_msg: - depth_msg.frame.visualize(self.display, timestamp=timestamp) image_np = depth_msg.frame.original_frame image_np = image_np[:, :, ::-1] self.pub["Depth"].publish(image_np) @@ -398,7 +328,6 @@ def on_watermark(self, timestamp): points[:,0] = -points[:,0] self.pub["DepthPointCloud"].publish(points) if self._flags.visualize_segmentation and segmentation_msg: - segmentation_msg.frame.visualize(self.display, timestamp=timestamp) self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) if self._flags.visualize_world: if prediction_camera_msg is None: @@ -424,11 +353,11 @@ def on_watermark(self, timestamp): lanes=lanes) self._planning_world.update_waypoints(None, waypoint_msg.waypoints) self._planning_world.draw_on_frame(frame) - frame.visualize(self.display, timestamp=timestamp) image_np = frame.as_rgb_numpy_array() self.pub["PlanningWorld"].publish(image_np) - self.render_text(pose_msg.data, control_msg, timestamp) + # todo: render text in foxglove + #self.render_text(pose_msg.data, control_msg, timestamp) def run(self): # Run method is invoked after all operators finished initializing. From 85cdf7314de19fddf764aca968b58d2165dcc195 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:21:19 -0400 Subject: [PATCH 34/47] removed run_visualizer_control_loop --- data_gatherer.py | 2 -- pylot.py | 2 -- pylot/utils.py | 27 --------------------------- 3 files changed, 31 deletions(-) diff --git a/data_gatherer.py b/data_gatherer.py index cc8adacc..46221b11 100644 --- a/data_gatherer.py +++ b/data_gatherer.py @@ -302,8 +302,6 @@ def main(argv): stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True))) try: - if pylot.flags.must_visualize(): - pylot.utils.run_visualizer_control_loop(control_display_stream) node_handle.wait() except KeyboardInterrupt: node_handle.shutdown() diff --git a/pylot.py b/pylot.py index 1d7665e7..cbd52793 100644 --- a/pylot.py +++ b/pylot.py @@ -260,8 +260,6 @@ def main(args): client.start_recorder(FLAGS.simulation_recording_file) node_handle, control_display_stream = driver() signal.signal(signal.SIGINT, shutdown) - if pylot.flags.must_visualize(): - pylot.utils.run_visualizer_control_loop(control_display_stream) node_handle.wait() except KeyboardInterrupt: shutdown_pylot(node_handle, client, world) diff --git a/pylot/utils.py b/pylot/utils.py index d3774e13..cffe6c43 100644 --- a/pylot/utils.py +++ b/pylot/utils.py @@ -1049,33 +1049,6 @@ def set_tf_loglevel(level): logging.getLogger('tensorflow').setLevel(level) -def run_visualizer_control_loop(control_display_stream): - """Runs a pygame loop that waits for user commands. - - The user commands are send on the control_display_stream - to control the pygame visualization window. - """ - import erdos - import pygame - clock = pygame.time.Clock() - from pygame.locals import K_n - while True: - clock.tick_busy_loop(60) - events = pygame.event.get() - for event in events: - if event.type == pygame.KEYUP: - if event.key == K_n: - control_display_stream.send( - erdos.Message(erdos.Timestamp(coordinates=[0]), - event.key)) - elif event.type == pygame.QUIT: - raise KeyboardInterrupt - elif event.type == pygame.KEYDOWN: - if (event.key == pygame.K_c - and pygame.key.get_mods() & pygame.KMOD_CTRL): - raise KeyboardInterrupt - - def verify_keys_in_dict(required_keys, arg_dict): assert set(required_keys).issubset(set(arg_dict.keys())), \ "one or more of {} not found in {}".format(required_keys, arg_dict) From 7702c6438299baddb1772f22c6215079c669403a Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:26:09 -0400 Subject: [PATCH 35/47] removed visualize() methods from camera_frame.py, depth_frame.py, point_cloud.py, and segmented_frame.py --- pylot/perception/camera_frame.py | 13 ------------- pylot/perception/depth_frame.py | 10 ---------- pylot/perception/point_cloud.py | 18 ------------------ .../perception/segmentation/segmented_frame.py | 9 --------- 4 files changed, 50 deletions(-) diff --git a/pylot/perception/camera_frame.py b/pylot/perception/camera_frame.py index 3e761f7e..9bfee860 100644 --- a/pylot/perception/camera_frame.py +++ b/pylot/perception/camera_frame.py @@ -137,19 +137,6 @@ def resize(self, width: int, height: int): dsize=(width, height), interpolation=cv2.INTER_NEAREST) - def visualize(self, pygame_display, timestamp=None): - """Visualizes the frame on a pygame display.""" - import pygame - if timestamp is not None: - pylot.utils.add_timestamp(self.frame, timestamp) - if self.encoding != 'RGB': - image_np = self.as_rgb_numpy_array() - else: - image_np = self.frame - image_np = np.transpose(image_np, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def save(self, timestamp: int, data_path: str, file_base: str): """Saves the camera frame to a file. diff --git a/pylot/perception/depth_frame.py b/pylot/perception/depth_frame.py index bf49987d..084ba2ca 100644 --- a/pylot/perception/depth_frame.py +++ b/pylot/perception/depth_frame.py @@ -128,16 +128,6 @@ def resize(self, width: int, height: int): dsize=(width, height), interpolation=cv2.INTER_NEAREST) - def visualize(self, pygame_display, timestamp=None): - """Visualizes the frame on a pygame display.""" - if self.original_frame is not None: - import pygame - image_np = self.original_frame - image_np = image_np[:, :, ::-1] - image_np = np.transpose(image_np, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def save(self, timestamp: int, data_path: str, file_base: str): """Saves the depth frame to a file. diff --git a/pylot/perception/point_cloud.py b/pylot/perception/point_cloud.py index 35c40997..3c426322 100644 --- a/pylot/perception/point_cloud.py +++ b/pylot/perception/point_cloud.py @@ -184,24 +184,6 @@ def save(self, timestamp: int, data_path: str, file_base: str): pcd.points = o3d.Vector3dVector(self.points) o3d.write_point_cloud(file_name, pcd) - def visualize(self, pygame_display, timestamp=None): - """Visualizes the point cloud on a pygame display.""" - import pygame - (width, height) = pygame_display.get_size() - # Transform point cloud to top down view. - lidar_data = np.array(self.global_points[:, :2]) - lidar_data *= (min(width, height) / - (2.0 * self._lidar_setup.get_range_in_meters())) - lidar_data += (0.5 * width, 0.5 * height) - lidar_data = np.fabs(lidar_data) - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (width, height, 3) - lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - pygame.surfarray.blit_array(pygame_display, lidar_img) - pygame.display.flip() - def __repr__(self): return 'PointCloud(lidar setup: {}, points: {})'.format( self._lidar_setup, self.points) diff --git a/pylot/perception/segmentation/segmented_frame.py b/pylot/perception/segmentation/segmented_frame.py index 2d793fe2..f87cf71d 100644 --- a/pylot/perception/segmentation/segmented_frame.py +++ b/pylot/perception/segmentation/segmented_frame.py @@ -274,15 +274,6 @@ def save(self, timestamp, data_path, file_base): img = Image.fromarray(self.as_cityscapes_palette()) img.save(file_name) - def visualize(self, pygame_display, timestamp=None): - import pygame - cityscapes_frame = self.as_cityscapes_palette() - if timestamp is not None: - pylot.utils.add_timestamp(cityscapes_frame, timestamp) - image_np = np.transpose(cityscapes_frame, (1, 0, 2)) - pygame.surfarray.blit_array(pygame_display, image_np) - pygame.display.flip() - def draw_box(self, start_point, end_point, color, thickness=3): """Draw a colored box defined by start_point, end_point.""" start = (int(start_point.x), int(start_point.y)) From 0fd5ad47da22b9df84838bf2bf4ec175385a37c1 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 5 Jul 2021 22:40:56 -0400 Subject: [PATCH 36/47] renamed ROSCameraPublisher.py to ros_camera_publisher.py and ROSLIDARPublisher.py to ros_lidar_publisher.py and adjusted import statement in visualizer operator --- .../debug/{ROSCameraPublisher.py => ros_camera_publisher.py} | 0 pylot/debug/{ROSLIDARPublisher.py => ros_lidar_publisher.py} | 0 pylot/debug/visualizer_operator.py | 4 ++-- 3 files changed, 2 insertions(+), 2 deletions(-) rename pylot/debug/{ROSCameraPublisher.py => ros_camera_publisher.py} (100%) rename pylot/debug/{ROSLIDARPublisher.py => ros_lidar_publisher.py} (100%) diff --git a/pylot/debug/ROSCameraPublisher.py b/pylot/debug/ros_camera_publisher.py similarity index 100% rename from pylot/debug/ROSCameraPublisher.py rename to pylot/debug/ros_camera_publisher.py diff --git a/pylot/debug/ROSLIDARPublisher.py b/pylot/debug/ros_lidar_publisher.py similarity index 100% rename from pylot/debug/ROSLIDARPublisher.py rename to pylot/debug/ros_lidar_publisher.py diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 5e1cd4b2..a7f3f351 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -17,8 +17,8 @@ DEFAULT_VIS_TIME = 30000.0 import rospy -from .ROSCameraPublisher import ROSCameraPublisher -from .ROSLIDARPublisher import ROSLIDARPublisher +from .ros_camera_publisher import ROSCameraPublisher +from .ros_lidar_publisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state From 56590a5bd9884b1cd67b9599e398cecc6edc66bd Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 6 Jul 2021 02:17:56 -0400 Subject: [PATCH 37/47] added updated dockerfile (WIP) to install foxglove and ROS --- docker/Dockerfile | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index 80991608..acef0a74 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -72,3 +72,30 @@ RUN echo "if [ -f ~/.bashrc ]; then . ~/.bashrc ; fi" >> ~/.bash_profile # Set up ssh access to the container. RUN cd ~/ && ssh-keygen -q -t rsa -N '' -f ~/.ssh/id_rsa <<&1 >/dev/null RUN sudo sed -i 's/#X11UseLocalhost yes/X11UseLocalhost no/g' /etc/ssh/sshd_config + +# WIP (for visualization in Foxglove) +# FOR NOW: install foxglove-studio and ROS manually in the pylot container + +# # Download Foxglove +# RUN cd /home/erdos/workspace/pylot +# RUN curl -L https://github.com/foxglove/studio/releases/download/v0.10.2/foxglove-studio-0.10.2-linux-amd64.deb -o foxglove-studio-0.10.2-linux-amd64.deb +# RUN sudo apt install ./foxglove-studio-*.deb + +# # Download ROS +# RUN cd /home/erdos/workspace/pylot +# # From, http://wiki.ros.org/melodic/Installation/Ubuntu: +# # Setup your computer to accept software from packages.ros.org: +# RUN sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +# # Set up your keys +# RUN sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +# # Make sure your Debian package index is up-to-date +# RUN sudo apt update +# # Desktop-Full Install +# RUN sudo apt install ros-melodic-desktop-full +# # Set up ROS environment variables +# RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc +# RUN source ~/.bashrc + + + + From 2206c7112f806c191d555fcb3590ad3675e1ea04 Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 13 Jul 2021 21:38:15 -0400 Subject: [PATCH 38/47] removed pygame depedencies from operator_creator.py --- pylot/operator_creator.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/pylot/operator_creator.py b/pylot/operator_creator.py index 2274901e..b4d23587 100644 --- a/pylot/operator_creator.py +++ b/pylot/operator_creator.py @@ -837,12 +837,6 @@ def add_visualizer(pose_stream=None, control_stream=None, name='visualizer_operator'): from pylot.debug.visualizer_operator import VisualizerOperator - import pygame - pygame.init() - pygame_display = pygame.display.set_mode( - (FLAGS.camera_image_width, FLAGS.camera_image_height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - pygame.display.set_caption("Pylot") streams_to_send_top_on = [] if pose_stream is None: pose_stream = erdos.IngestStream() @@ -911,7 +905,7 @@ def add_visualizer(pose_stream=None, obstacles_stream, traffic_lights_stream, tracked_obstacles_stream, lane_detection_stream, prediction_stream, waypoints_stream, control_stream, control_display_stream - ], pygame_display, FLAGS) + ], FLAGS) return control_display_stream, streams_to_send_top_on From 810148325d6023d89fac1453b46ac3a770cac69e Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 18 Jul 2021 23:31:25 -0400 Subject: [PATCH 39/47] added docstrings to ROSCameraPublisher and ROSLIDARPublisher classes and methods, and added comments to visualizer operator --- pylot/debug/ros_camera_publisher.py | 16 +++++++++++++--- pylot/debug/ros_lidar_publisher.py | 15 ++++++++++++++- pylot/debug/visualizer_operator.py | 4 +++- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/pylot/debug/ros_camera_publisher.py b/pylot/debug/ros_camera_publisher.py index 84a49256..75872dae 100644 --- a/pylot/debug/ros_camera_publisher.py +++ b/pylot/debug/ros_camera_publisher.py @@ -4,14 +4,24 @@ from sensor_msgs.msg import Image class ROSCameraPublisher: + """Class that stores a ROS publisher node that publishes ROS Image messages + + Args: + topic: the name of the topic published to + + Attributes: + image_pub: ROS publisher node + """ def __init__(self, topic:str): - # publishes to the given topic self.image_pub = rospy.Publisher(topic, Image, queue_size=10) def publish(self, img_arr): - # converts the 3d np arrary img_arr to a sensor_msgs/Image datatype - # inspired by https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py + """Publishes a sensor_msgs/Image message (constructed from input) + + Args: + img_arr: A numpy array storing a frame (e.g. camera, depth, etc.) + """ img_msg = Image(encoding='rgb8') if type(img_arr[0][0][0]) != np.int8: img_arr = img_arr.astype(np.int8) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index ef68c126..d655fe1f 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -4,12 +4,25 @@ from sensor_msgs.msg import PointCloud2, PointField class ROSLIDARPublisher: + """Class that stores a ROS publisher node that publishes ROS Point Cloud messages + + Args: + topic: the name of the topic published to + + Attributes: + point_cloud_pub: ROS publisher node + """ def __init__(self, topic:str): - # publishes to the given topic self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) def publish(self, points): + """Publishes a sensor_msgs/PointCloud2 message (constructed from input) + + Args: + points: A numpy array storing a point cloud (see pylot.pylot.perception.point_cloud) + """ + # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) points_byte_array = points.tobytes() diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index a7f3f351..81f3c7ec 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -8,6 +8,7 @@ import erdos import numpy as np +import rospy import pylot.utils from pylot.drivers.sensor_setup import RGBCameraSetup @@ -16,7 +17,6 @@ DEFAULT_VIS_TIME = 30000.0 -import rospy from .ros_camera_publisher import ROSCameraPublisher from .ros_lidar_publisher import ROSLIDARPublisher @@ -309,6 +309,8 @@ def on_watermark(self, timestamp): obstacle_prediction.draw_trajectory_on_frame(frame) if self._flags.visualize_lidar and point_cloud_msg: points = point_cloud_msg.point_cloud.points + # need to switch and reverse axes (from Velodyne coordinate space) + # for correct visualization in foxglove points[:,[0,2]] = points[:,[2,0]] points[:,[1,2]] = points[:,[2,1]] points[:,0] = -points[:,0] From 4b5cab51c9973ba5fdc5ee76d40122fa830eb18a Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:10:24 -0400 Subject: [PATCH 40/47] minor comment syntax fix Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 81f3c7ec..2866e9d3 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -358,8 +358,8 @@ def on_watermark(self, timestamp): image_np = frame.as_rgb_numpy_array() self.pub["PlanningWorld"].publish(image_np) - # todo: render text in foxglove - #self.render_text(pose_msg.data, control_msg, timestamp) + # TODO: render text in foxglove + # self.render_text(pose_msg.data, control_msg, timestamp) def run(self): # Run method is invoked after all operators finished initializing. From af7fac502694fdf6822e4f2066430879dc2afc4e Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:10:47 -0400 Subject: [PATCH 41/47] minor comment syntax fix Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 2866e9d3..6806fae8 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -132,7 +132,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) - # PYLOT-ROS Integration + # Pylot-ROS Integration for Foxglove visualization rospy.init_node("visualizer", anonymous=True, disable_signals=True) self.pub = {} # dict of publishers From 08f945e392a4a5d8a6cb2fd23459c016a1a50762 Mon Sep 17 00:00:00 2001 From: storm-light Date: Sun, 18 Jul 2021 23:11:10 -0400 Subject: [PATCH 42/47] changed import statements to include full path Co-authored-by: Peter Schafhalter --- pylot/debug/visualizer_operator.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 6806fae8..26297801 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -17,8 +17,9 @@ DEFAULT_VIS_TIME = 30000.0 -from .ros_camera_publisher import ROSCameraPublisher -from .ros_lidar_publisher import ROSLIDARPublisher +import rospy +from pylot.debug.ros_camera_publisher import ROSCameraPublisher +from pylot.debug.ros_lidar_publisher import ROSLIDARPublisher class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state From 9aebae6fb92f5ceeb9f0f52ff9f656252a41b2a3 Mon Sep 17 00:00:00 2001 From: Josh Date: Sun, 18 Jul 2021 23:42:39 -0400 Subject: [PATCH 43/47] minor comment changes --- pylot/debug/ros_lidar_publisher.py | 1 - 1 file changed, 1 deletion(-) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index d655fe1f..5163e9ae 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -23,7 +23,6 @@ def publish(self, points): points: A numpy array storing a point cloud (see pylot.pylot.perception.point_cloud) """ - # converts point cloud points array to a sensor_msgs/PointCloud2 datatype points = points.astype(np.float32) points_byte_array = points.tobytes() row_step = len(points_byte_array) From 2074851720d0b034a3657d9f9fa42cb595d67774 Mon Sep 17 00:00:00 2001 From: Josh Date: Mon, 19 Jul 2021 02:21:45 -0400 Subject: [PATCH 44/47] yapf'd deez files --- pylot/debug/ros_camera_publisher.py | 15 +++++++-------- pylot/debug/ros_lidar_publisher.py | 16 ++++++++++------ pylot/debug/visualizer_operator.py | 28 ++++++++++++++++------------ 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/pylot/debug/ros_camera_publisher.py b/pylot/debug/ros_camera_publisher.py index 75872dae..83c90ada 100644 --- a/pylot/debug/ros_camera_publisher.py +++ b/pylot/debug/ros_camera_publisher.py @@ -3,6 +3,7 @@ import numpy as np from sensor_msgs.msg import Image + class ROSCameraPublisher: """Class that stores a ROS publisher node that publishes ROS Image messages @@ -12,10 +13,9 @@ class ROSCameraPublisher: Attributes: image_pub: ROS publisher node """ - - def __init__(self, topic:str): + def __init__(self, topic: str): self.image_pub = rospy.Publisher(topic, Image, queue_size=10) - + def publish(self, img_arr): """Publishes a sensor_msgs/Image message (constructed from input) @@ -27,9 +27,8 @@ def publish(self, img_arr): img_arr = img_arr.astype(np.int8) img_msg.height, img_msg.width, channels = img_arr.shape img_msg.data = img_arr.tobytes() - img_msg.step = img_msg.width * img_msg.height - img_msg.is_bigendian = ( - img_arr.dtype.byteorder == '>' or - img_arr.dtype.byteorder == '=' and sys.byteorder == 'big' - ) + img_msg.step = img_msg.width * img_msg.height + img_msg.is_bigendian = (img_arr.dtype.byteorder == '>' + or img_arr.dtype.byteorder == '=' + and sys.byteorder == 'big') self.image_pub.publish(img_msg) diff --git a/pylot/debug/ros_lidar_publisher.py b/pylot/debug/ros_lidar_publisher.py index 5163e9ae..37449887 100644 --- a/pylot/debug/ros_lidar_publisher.py +++ b/pylot/debug/ros_lidar_publisher.py @@ -3,6 +3,7 @@ import numpy as np from sensor_msgs.msg import PointCloud2, PointField + class ROSLIDARPublisher: """Class that stores a ROS publisher node that publishes ROS Point Cloud messages @@ -12,10 +13,11 @@ class ROSLIDARPublisher: Attributes: point_cloud_pub: ROS publisher node """ + def __init__(self, topic: str): + self.point_cloud_pub = rospy.Publisher(topic, + PointCloud2, + queue_size=10) - def __init__(self, topic:str): - self.point_cloud_pub = rospy.Publisher(topic, PointCloud2, queue_size=10) - def publish(self, points): """Publishes a sensor_msgs/PointCloud2 message (constructed from input) @@ -27,9 +29,11 @@ def publish(self, points): points_byte_array = points.tobytes() row_step = len(points_byte_array) point_step = len(points[0].tobytes()) - fields = [PointField('x', 0, PointField.FLOAT32, 1), - PointField('y', 4, PointField.FLOAT32, 1), - PointField('z', 8, PointField.FLOAT32, 1)] + fields = [ + PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1) + ] point_cloud_msg = PointCloud2(height=1, width=len(points), is_dense=True, diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 26297801..2fbda67d 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -135,7 +135,7 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, # Pylot-ROS Integration for Foxglove visualization rospy.init_node("visualizer", anonymous=True, disable_signals=True) - self.pub = {} # dict of publishers + self.pub = {} # dict of publishers # Creating ROS publishers for streams to be visualized if flags.visualize_rgb_camera: @@ -143,7 +143,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, if flags.visualize_detected_obstacles: self.pub["Obstacle"] = ROSCameraPublisher("/camera/obstacle") if flags.visualize_tracked_obstacles: - self.pub["TrackedObstacle"] = ROSCameraPublisher("/camera/tracked_obstacle") + self.pub["TrackedObstacle"] = ROSCameraPublisher( + "/camera/tracked_obstacle") if flags.visualize_detected_traffic_lights: self.pub["TLCamera"] = ROSCameraPublisher("/camera/tl_camera") if flags.visualize_waypoints: @@ -156,7 +157,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self.pub["Depth"] = ROSCameraPublisher("/camera/depth") self.pub["DepthPointCloud"] = ROSLIDARPublisher("/lidar/depth") if flags.visualize_segmentation: - self.pub["Segmentation"] = ROSCameraPublisher("/camera/segmentation") + self.pub["Segmentation"] = ROSCameraPublisher( + "/camera/segmentation") if flags.visualize_world: self._planning_world = World(flags, self._logger) top_down_transform = pylot.utils.get_top_down_transform( @@ -166,7 +168,8 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self._bird_eye_camera_setup = RGBCameraSetup( 'bird_eye_camera', flags.camera_image_width, flags.camera_image_height, top_down_transform, 90) - self.pub["PlanningWorld"] = ROSCameraPublisher("/camera/planning_world") + self.pub["PlanningWorld"] = ROSCameraPublisher( + "/camera/planning_world") else: self._planning_world = None @@ -312,10 +315,10 @@ def on_watermark(self, timestamp): points = point_cloud_msg.point_cloud.points # need to switch and reverse axes (from Velodyne coordinate space) # for correct visualization in foxglove - points[:,[0,2]] = points[:,[2,0]] - points[:,[1,2]] = points[:,[2,1]] - points[:,0] = -points[:,0] - points[:,2] = -points[:,2] + points[:, [0, 2]] = points[:, [2, 0]] + points[:, [1, 2]] = points[:, [2, 1]] + points[:, 0] = -points[:, 0] + points[:, 2] = -points[:, 2] self.pub["PointCloud"].publish(points) if self._flags.visualize_detected_lanes and bgr_msg and lane_detection_msg: for lane in lane_detection_msg.data: @@ -328,17 +331,18 @@ def on_watermark(self, timestamp): self.pub["Depth"].publish(image_np) depth_msg.frame.resize(854, 480) points = depth_msg.frame.as_point_cloud() - points[:,0] = -points[:,0] + points[:, 0] = -points[:, 0] self.pub["DepthPointCloud"].publish(points) if self._flags.visualize_segmentation and segmentation_msg: - self.pub["Segmentation"].publish(segmentation_msg.frame.as_cityscapes_palette()) + self.pub["Segmentation"].publish( + segmentation_msg.frame.as_cityscapes_palette()) if self._flags.visualize_world: if prediction_camera_msg is None: # Top-down prediction is not available. Show planning # world on a black image. black_img = np.zeros((self._bird_eye_camera_setup.height, - self._bird_eye_camera_setup.width, 3), - dtype=np.dtype("uint8")) + self._bird_eye_camera_setup.width, 3), + dtype=np.dtype("uint8")) frame = CameraFrame(black_img, 'RGB', self._bird_eye_camera_setup) else: From 053cdabc5c7b049b4eb54bc91489d50b363c64e0 Mon Sep 17 00:00:00 2001 From: Josh Date: Tue, 20 Jul 2021 00:38:31 -0400 Subject: [PATCH 45/47] removed extra import statements --- pylot/debug/visualizer_operator.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index 20cdb6e7..f9d27ad1 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -19,10 +19,6 @@ DEFAULT_VIS_TIME = 30000.0 -import rospy -from pylot.debug.ros_camera_publisher import ROSCameraPublisher -from pylot.debug.ros_lidar_publisher import ROSLIDARPublisher - class VisualizerOperator(erdos.Operator): """ The `VisualizerOperator` allows developers to see the current state of the entire pipeline by visualizing it in Foxglove Studio. From 72ecd9ad6a9e5d8e15790ce1cebf772c9c838359 Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 22 Jul 2021 01:47:10 -0400 Subject: [PATCH 46/47] runs roscore from visualizer operator's init() and kills process in destroy() --- pylot/debug/visualizer_operator.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/pylot/debug/visualizer_operator.py b/pylot/debug/visualizer_operator.py index f9d27ad1..0cea2ada 100644 --- a/pylot/debug/visualizer_operator.py +++ b/pylot/debug/visualizer_operator.py @@ -9,6 +9,7 @@ import numpy as np import rospy +import subprocess import pylot.utils from pylot.drivers.sensor_setup import RGBCameraSetup @@ -132,6 +133,9 @@ def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream, self.config.log_file_name) # Pylot-ROS Integration for Foxglove visualization + self.roscore = subprocess.Popen('roscore') + import time + time.sleep(1) rospy.init_node("visualizer", anonymous=True, disable_signals=True) self.pub = {} # dict of publishers @@ -185,6 +189,7 @@ def connect(pose_stream, rgb_camera_stream, tl_camera_stream, def destroy(self): self._logger.warn('destroying {}'.format(self.config.name)) + self.roscore.terminate() def save(self, msg, msg_type, queue): self._logger.debug("@{}: Received {} message.".format( From 45436d7e7628b281b924eb3f97f2b18191c93f28 Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 22 Jul 2021 02:00:11 -0400 Subject: [PATCH 47/47] updated Dockerfile to correctly install foxglove and ros --- docker/Dockerfile | 51 ++++++++++++++++++++++++++--------------------- 1 file changed, 28 insertions(+), 23 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index acef0a74..f7c092ca 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -74,28 +74,33 @@ RUN cd ~/ && ssh-keygen -q -t rsa -N '' -f ~/.ssh/id_rsa <<&1 >/dev/null RUN sudo sed -i 's/#X11UseLocalhost yes/X11UseLocalhost no/g' /etc/ssh/sshd_config # WIP (for visualization in Foxglove) -# FOR NOW: install foxglove-studio and ROS manually in the pylot container - -# # Download Foxglove -# RUN cd /home/erdos/workspace/pylot -# RUN curl -L https://github.com/foxglove/studio/releases/download/v0.10.2/foxglove-studio-0.10.2-linux-amd64.deb -o foxglove-studio-0.10.2-linux-amd64.deb -# RUN sudo apt install ./foxglove-studio-*.deb - -# # Download ROS -# RUN cd /home/erdos/workspace/pylot -# # From, http://wiki.ros.org/melodic/Installation/Ubuntu: -# # Setup your computer to accept software from packages.ros.org: -# RUN sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -# # Set up your keys -# RUN sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 -# # Make sure your Debian package index is up-to-date -# RUN sudo apt update -# # Desktop-Full Install -# RUN sudo apt install ros-melodic-desktop-full -# # Set up ROS environment variables -# RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc -# RUN source ~/.bashrc - - +# Set up for Foxglove +RUN sudo apt-get -y install wget + +# Install Foxglove Studio +RUN cd /tmp +RUN wget https://github.com/foxglove/studio/releases/download/v0.12.0/foxglove-studio-0.12.0-linux-amd64.deb +RUN sudo apt install -y ./foxglove-studio-*.deb +RUN rm ./foxglove-studio-*.deb + +# Set up for ROS +RUN sudo apt -y update +RUN sudo apt install -y lsb-release +RUN sudo apt install -y curl + +# Download ROS +# From, http://wiki.ros.org/melodic/Installation/Ubuntu: +# Setup your computer to accept software from packages.ros.org: +RUN sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +# Set up your keys +# RUN sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - +# Make sure your Debian package index is up-to-date +RUN sudo apt -y update +# Desktop-Full Install +RUN sudo apt -y install ros-melodic-desktop-full +# Set up ROS environment variables +RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc +RUN source ~/.bashrc