def on_watermark(self, timestamp, obstacles_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
depth_msg = self._depth_frame_msgs.popleft()
bgr_msg = self._bgr_msgs.popleft()
segmented_msg = self._segmented_msgs.popleft()
pose_msg = self._pose_msgs.popleft()
obstacles_msg = self._obstacles.popleft()
speed_limit_signs_msg = self._speed_limit_signs.popleft()
stop_signs_msg = self._stop_signs.popleft()
self._frame_cnt += 1
if (hasattr(self._flags, 'log_every_nth_message')
and self._frame_cnt % self._flags.log_every_nth_message != 0):
# There's no point to run the perfect detector if collecting
# data, and only logging every nth frame.
obstacles_stream.send(ObstaclesMessage(timestamp, []))
return
vehicle_transform = pose_msg.data.transform
# The camera setup sent with the image is relative to the car, we need
# to transform it relative to the world.
depth_msg.frame.camera_setup.set_transform(
vehicle_transform * depth_msg.frame.camera_setup.transform)
det_obstacles = self.__get_obstacles(obstacles_msg.obstacles,
vehicle_transform,
depth_msg.frame,
segmented_msg.frame)
det_speed_limits = pylot.simulation.utils.get_detected_speed_limits(
speed_limit_signs_msg.speed_signs, depth_msg.frame,
segmented_msg.frame)
det_stop_signs = pylot.simulation.utils.get_detected_traffic_stops(
stop_signs_msg.stop_signs, depth_msg.frame)
det_obstacles = det_obstacles + det_speed_limits + det_stop_signs
# Send the detected obstacles.
obstacles_stream.send(ObstaclesMessage(timestamp, det_obstacles))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
bgr_msg.frame.annotate_with_bounding_boxes(bgr_msg.timestamp,
det_obstacles,
vehicle_transform)
if self._flags.visualize_detected_obstacles:
bgr_msg.frame.visualize(
self.config.name,
pygame_display=pylot.utils.PYGAME_DISPLAY)
if self._flags.log_detector_output:
bgr_msg.frame.save(bgr_msg.timestamp.coordinates[0],
self._flags.data_path, 'perfect-detector')
def on_watermark(self, timestamp, obstacles_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
depth_msg = self._depth_frame_msgs.popleft()
bgr_msg = self._bgr_msgs.popleft()
segmented_msg = self._segmented_msgs.popleft()
can_bus_msg = self._can_bus_msgs.popleft()
obstacles_msg = self._obstacles.popleft()
speed_limit_signs_msg = self._speed_limit_signs.popleft()
stop_signs_msg = self._stop_signs.popleft()
self._frame_cnt += 1
if (hasattr(self._flags, 'log_every_nth_message')
and self._frame_cnt % self._flags.log_every_nth_message != 0):
# There's no point to run the perfect detector if collecting
# data, and only logging every nth frame.
obstacles_stream.send(ObstaclesMessage([], timestamp))
return
vehicle_transform = can_bus_msg.data.transform
# The camera setup sent with the image is relative to the car, we need
# to transform it relative to the world.
depth_msg.frame.camera_setup.set_transform(
vehicle_transform * depth_msg.frame.camera_setup.transform)
det_obstacles = self.__get_obstacles(obstacles_msg.obstacles,
vehicle_transform,
depth_msg.frame,
segmented_msg.frame)
det_speed_limits = pylot.simulation.utils.get_detected_speed_limits(
speed_limit_signs_msg.speed_signs, depth_msg.frame,
segmented_msg.frame)
det_stop_signs = pylot.simulation.utils.get_detected_traffic_stops(
stop_signs_msg.stop_signs, depth_msg.frame)
det_obstacles = det_obstacles + det_speed_limits + det_stop_signs
# Send the detected obstacles.
obstacles_stream.send(ObstaclesMessage(det_obstacles, timestamp))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
bgr_msg.frame.annotate_with_bounding_boxes(bgr_msg.timestamp,
det_obstacles)
if self._flags.visualize_detected_obstacles:
bgr_msg.frame.visualize(self._name)
if self._flags.log_detector_output:
bgr_msg.frame.save(bgr_msg.timestamp.coordinates[0],
self._flags.data_path, 'perfect-detector')
def __send_ground_actors_data(self, timestamp: Timestamp):
# Get all the actors in the simulation.
actor_list = self._world.get_actors()
(vehicles, people, traffic_lights, speed_limits, traffic_stops
) = pylot.simulation.utils.extract_data_in_pylot_format(actor_list)
# Send ground people and vehicles.
self.ground_obstacles_stream.send(
ObstaclesMessage(timestamp, vehicles + people))
self.ground_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
# Send ground traffic lights.
self.ground_traffic_lights_stream.send(
TrafficLightsMessage(timestamp, traffic_lights))
self.ground_traffic_lights_stream.send(
erdos.WatermarkMessage(timestamp))
# Send ground speed signs.
self.ground_speed_limit_signs_stream.send(
SpeedSignsMessage(timestamp, speed_limits))
self.ground_speed_limit_signs_stream.send(
erdos.WatermarkMessage(timestamp))
# Send stop signs.
self.ground_stop_signs_stream.send(
StopSignsMessage(timestamp, traffic_stops))
self.ground_stop_signs_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, obstacle_tracking_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
frame_msg = self._frame_msgs.popleft()
camera_frame = frame_msg.frame
tracked_obstacles = []
self._watermark_msg_count += 1
if len(self._obstacles_msgs) > 0:
obstacles_msg = self._obstacles_msgs.popleft()
assert frame_msg.timestamp == obstacles_msg.timestamp
detected_obstacles = []
for obstacle in obstacles_msg.obstacles:
if obstacle.is_vehicle() or obstacle.is_person():
detected_obstacles.append(obstacle)
if (self._watermark_msg_count %
self._flags.track_every_nth_detection == 0):
# Reinitialize the tracker with new detections.
self._logger.debug(
'Restarting trackers at frame {}'.format(timestamp))
self._tracker.reinitialize(camera_frame, detected_obstacles)
self._logger.debug('Processing frame {}'.format(timestamp))
ok, tracked_obstacles = self._tracker.track(camera_frame)
if not ok:
self._logger.error(
'Tracker failed at timestamp {}'.format(timestamp))
obstacle_tracking_stream.send(
ObstaclesMessage(timestamp, tracked_obstacles, 0))
obstacle_tracking_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, obstacles_output_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._pose_msgs.popleft().data.transform
frame_msg = self._frame_msgs.popleft()
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
self._logger.info('@{}: {}'.format(timestamp, obstacles_with_location))
if self._flags.visualize_obstacles_with_distance:
frame_msg.frame.annotate_with_bounding_boxes(
timestamp, obstacles_with_location, vehicle_transform)
frame_msg.frame.visualize(
self.config.name, pygame_display=pylot.utils.PYGAME_DISPLAY)
obstacles_output_stream.send(
ObstaclesMessage(timestamp, obstacles_with_location))
def main(argv):
(obstacles_stream, traffic_lights_stream, obstacles_tracking_stream,
open_drive_stream, global_trajectory_stream) = create_data_flow()
# Run the data-flow.
erdos.run_async()
top_timestamp = erdos.Timestamp(coordinates=[sys.maxsize])
open_drive_stream.send(erdos.WatermarkMessage(top_timestamp))
waypoints = [[waypoint] for waypoint in read_waypoints()]
global_trajectory_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]), waypoints))
global_trajectory_stream.send(erdos.WatermarkMessage(top_timestamp))
time_to_sleep = 1.0 / FLAGS.sensor_frequency
count = 0
while True:
timestamp = erdos.Timestamp(coordinates=[count])
if not FLAGS.obstacle_detection:
obstacles_stream.send(ObstaclesMessage(timestamp, []))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.traffic_light_detection:
traffic_lights_stream.send(TrafficLightsMessage(timestamp, []))
traffic_lights_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.obstacle_tracking:
obstacles_tracking_stream.send(
ObstacleTrajectoriesMessage(timestamp, []))
obstacles_tracking_stream.send(erdos.WatermarkMessage(timestamp))
count += 1
# NOTE: We should offset sleep time by the time it takes to send the
# messages.
time.sleep(time_to_sleep)
def on_watermark(self, timestamp, obstacle_tracking_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
frame_msg = self._frame_msgs.popleft()
camera_frame = frame_msg.frame
tracked_obstacles = []
if len(self._obstacles_msgs) > 0:
obstacles_msg = self._obstacles_msgs.popleft()
assert frame_msg.timestamp == obstacles_msg.timestamp
self._logger.debug(
'Restarting trackers at frame {}'.format(timestamp))
detected_obstacles = []
for obstacle in obstacles_msg.obstacles:
if (obstacle.label in VEHICLE_LABELS
or obstacle.label == 'person'):
detected_obstacles.append(obstacle)
self._tracker.reinitialize(camera_frame, detected_obstacles)
self._logger.debug('Processing frame {}'.format(timestamp))
ok, tracked_obstacles = self._tracker.track(camera_frame)
if not ok:
self._logger.error(
'Tracker failed at timestamp {}'.format(timestamp))
obstacle_tracking_stream.send(
ObstaclesMessage(timestamp, tracked_obstacles, 0))
if self._flags.visualize_tracker_output:
# Tracked obstacles have no label, draw white bbox.
camera_frame.annotate_with_bounding_boxes(timestamp,
tracked_obstacles)
camera_frame.visualize(self.config.name,
pygame_display=pylot.utils.PYGAME_DISPLAY)
def send_perfect_detections(self, perfect_obstacles_stream,
perfect_traffic_lights_stream, timestamp,
tl_camera_location):
"""Send perfect detections for agents and traffic lights.
This method first connects to the simulator to extract all the
agents and traffic light in a scenario. Next, it transforms them into
the types Pylot expects, and sends them on the streams for perfect
detections.
Note: This is only used when executing using a perfect perception
component.
"""
if not (FLAGS.simulator_obstacle_detection
or FLAGS.simulator_traffic_light_detection
or FLAGS.evaluate_obstacle_detection
or FLAGS.evaluate_obstacle_tracking):
return
from pylot.simulation.utils import extract_data_in_pylot_format
actor_list = self._world.get_actors()
(vehicles, people, traffic_lights, _,
_) = extract_data_in_pylot_format(actor_list)
if (FLAGS.simulator_obstacle_detection
or FLAGS.evaluate_obstacle_detection
or FLAGS.evaluate_obstacle_tracking):
perfect_obstacles_stream.send(
ObstaclesMessage(timestamp, vehicles + people))
perfect_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if FLAGS.simulator_traffic_light_detection:
vec_transform = pylot.utils.Transform.from_simulator_transform(
self._ego_vehicle.get_transform())
tl_camera_transform = pylot.utils.Transform(
tl_camera_location, pylot.utils.Rotation())
visible_tls = []
if self._town_name is None:
self._town_name = self._world.get_map().name
for tl in traffic_lights:
if tl.is_traffic_light_visible(
vec_transform * tl_camera_transform,
self._town_name,
distance_threshold=FLAGS.
static_obstacle_distance_threshold):
if self._town_name not in ['Town01', 'Town02']:
delta_y = -5
if self._town_name == 'Town04':
delta_y = -2
# Move the traffic light location to the road.
tl.transform = tl.transform * pylot.utils.Transform(
pylot.utils.Location(delta_y, 0, 5),
pylot.utils.Rotation())
visible_tls.append(tl)
perfect_traffic_lights_stream.send(
TrafficLightsMessage(timestamp, visible_tls))
perfect_traffic_lights_stream.send(
erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, traffic_lights_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
traffic_light_msg = self._traffic_lights.popleft()
bgr_msg = self._bgr_msgs.popleft()
depth_msg = self._depth_frame_msgs.popleft()
segmented_msg = self._segmented_msgs.popleft()
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._frame_cnt += 1
if (hasattr(self._flags, 'log_every_nth_message')
and self._frame_cnt % self._flags.log_every_nth_message != 0):
# There's no point to run the perfect detector if collecting
# data, and only logging every nth frame.
traffic_lights_stream.send(ObstaclesMessage([], timestamp))
return
# The camera setup sent with the image is relative to the car, we need
# to transform it relative to the world to detect traffic lights.
depth_msg.frame.camera_setup.set_transform(
vehicle_transform * depth_msg.frame.camera_setup.transform)
det_traffic_lights = get_traffic_lights_obstacles(
traffic_light_msg.traffic_lights, depth_msg.frame,
segmented_msg.frame, self._town_name)
if (self._flags.visualize_detected_traffic_lights
or self._flags.log_detector_output):
bgr_msg.frame.annotate_with_bounding_boxes(bgr_msg.timestamp,
det_traffic_lights)
if self._flags.visualize_detected_traffic_lights:
bgr_msg.frame.visualize(self._name)
if self._flags.log_detector_output:
bgr_msg.frame.save(bgr_msg.timestamp.coordinates[0],
self._flags.data_path, 'perfect-detector')
# Send the detected traffic lights.
traffic_lights_stream.send(
ObstaclesMessage(det_traffic_lights, timestamp))
def on_frame(self, msg, traffic_lights_stream):
""" Invoked when the operator receives a message on the data stream."""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(msg.frame.as_rgb_numpy_array(),
axis=0)
(boxes, scores, classes, num) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num[0])
labels = [self._labels[label] for label in classes[0][:num_detections]]
boxes = boxes[0][:num_detections]
scores = scores[0][:num_detections]
traffic_lights = self.__convert_to_detected_tl(
boxes, scores, labels, msg.frame.camera_setup.height,
msg.frame.camera_setup.width)
self._logger.debug('@{}: {} detected traffic lights {}'.format(
msg.timestamp, self._name, traffic_lights))
if (self._flags.visualize_detected_traffic_lights
or self._flags.log_traffic_light_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp,
traffic_lights,
self._bbox_colors)
if self._flags.visualize_detected_traffic_lights:
msg.frame.visualize(self._name)
if self._flags.log_traffic_light_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'tl-detector-{}'.format(self._name))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
traffic_lights_stream.send(
ObstaclesMessage(traffic_lights, msg.timestamp, runtime))
def __publish_ground_actors_data(self, timestamp, watermark_msg):
# Get all the actors in the simulation.
actor_list = self._world.get_actors()
(vehicles, people, traffic_lights, speed_limits,
traffic_stops) = extract_data_in_pylot_format(actor_list)
obstacles_msg = ObstaclesMessage(timestamp, vehicles + people)
self._ground_obstacles_stream.send(obstacles_msg)
self._ground_obstacles_stream.send(watermark_msg)
traffic_lights_msg = TrafficLightsMessage(timestamp, traffic_lights)
self._ground_traffic_lights_stream.send(traffic_lights_msg)
self._ground_traffic_lights_stream.send(watermark_msg)
speed_limit_signs_msg = SpeedSignsMessage(timestamp, speed_limits)
self._ground_speed_limit_signs_stream.send(speed_limit_signs_msg)
self._ground_speed_limit_signs_stream.send(watermark_msg)
stop_signs_msg = StopSignsMessage(timestamp, traffic_stops)
self._ground_stop_signs_stream.send(stop_signs_msg)
self._ground_stop_signs_stream.send(watermark_msg)
def on_watermark(self, timestamp, obstacles_output_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._pose_msgs.popleft().data.transform
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
self._logger.debug('@{}: {}'.format(timestamp,
obstacles_with_location))
obstacles_output_stream.send(
ObstaclesMessage(timestamp, obstacles_with_location))
def send_ground_data(self, timestamp):
if not (FLAGS.carla_obstacle_detection
or FLAGS.carla_traffic_light_detection):
return
actor_list = self._world.get_actors()
(vehicles, people, traffic_lights, _,
_) = pylot.simulation.utils.extract_data_in_pylot_format(actor_list)
if FLAGS.carla_obstacle_detection:
self._ground_obstacles_stream.send(
ObstaclesMessage(timestamp, vehicles + people))
self._ground_obstacles_stream.send(
erdos.WatermarkMessage(timestamp))
if FLAGS.carla_traffic_light_detection:
vec_transform = pylot.utils.Transform.from_carla_transform(
self._ego_vehicle.get_transform())
tl_camera_transform = pylot.utils.Transform(
CENTER_CAMERA_LOCATION, pylot.utils.Rotation())
visible_tls = []
if self._town_name is None:
self._town_name = self._world.get_map().name
for tl in traffic_lights:
if tl.is_traffic_light_visible(
vec_transform * tl_camera_transform,
self._town_name,
distance_threshold=FLAGS.
static_obstacle_distance_threshold):
if self._town_name not in ['Town01', 'Town02']:
delta_y = -5
if self._town_name == 'Town04':
delta_y = -2
# Move the traffic light location to the road.
tl.transform = tl.transform * pylot.utils.Transform(
pylot.utils.Location(delta_y, 0, 5),
pylot.utils.Rotation())
visible_tls.append(tl)
self._ground_traffic_lights_stream.send(
TrafficLightsMessage(timestamp, visible_tls))
self._ground_traffic_lights_stream.send(
erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, obstacle_tracking_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
frame_msg = self._frame_msgs.popleft()
camera_frame = frame_msg.frame
tracked_obstacles = []
detector_runtime = 0
reinit_runtime = 0
# Check if the most recent obstacle message has this timestamp.
# If it doesn't, then the detector might have skipped sending
# an obstacle message.
if (len(self._obstacles_msgs) > 0
and self._obstacles_msgs[0].timestamp == timestamp):
obstacles_msg = self._obstacles_msgs.popleft()
self._detection_update_count += 1
if (self._detection_update_count %
self._flags.track_every_nth_detection == 0):
# Reinitialize the tracker with new detections.
self._logger.debug(
'Restarting trackers at frame {}'.format(timestamp))
detected_obstacles = []
for obstacle in obstacles_msg.obstacles:
if obstacle.is_vehicle() or obstacle.is_person():
detected_obstacles.append(obstacle)
reinit_runtime, _ = self._reinit_tracker(
camera_frame, detected_obstacles)
detector_runtime = obstacles_msg.runtime
tracker_runtime, (ok, tracked_obstacles) = \
self._run_tracker(camera_frame)
assert ok, 'Tracker failed at timestamp {}'.format(timestamp)
tracker_runtime = tracker_runtime + reinit_runtime
tracker_delay = self.__compute_tracker_delay(timestamp.coordinates[0],
detector_runtime,
tracker_runtime)
obstacle_tracking_stream.send(
ObstaclesMessage(timestamp, tracked_obstacles, tracker_delay))
def on_frame_msg(self, msg, obstacle_tracking_stream):
"""Invoked when a FrameMessage is received on the camera stream."""
self._logger.debug('@{}: {} received frame'.format(
msg.timestamp, self.config.name))
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
image_np = msg.frame.as_bgr_numpy_array()
results = self.run_model(image_np)
obstacles = []
for res in results:
track_id = res['tracking_id']
bbox = res['bbox']
score = res['score']
(label_id, ) = res['class'] - 1,
if label_id > 80:
continue
label = self.trained_dataset.class_name[label_id]
if label in ['Pedestrian', 'pedestrian']:
label = 'person'
elif label == 'Car':
label = 'car'
elif label == 'Cyclist':
label == 'bicycle'
if label in OBSTACLE_LABELS:
bounding_box_2D = BoundingBox2D(bbox[0], bbox[2], bbox[1],
bbox[3])
bounding_box_3D = None
if 'dim' in res and 'loc' in res and 'rot_y' in res:
bounding_box_3D = BoundingBox3D.from_dimensions(
res['dim'], res['loc'], res['rot_y'])
obstacles.append(
Obstacle(bounding_box_3D,
score,
label,
track_id,
bounding_box_2D=bounding_box_2D))
obstacle_tracking_stream.send(
ObstaclesMessage(msg.timestamp, obstacles, 0))
def on_msg_camera_stream(self, msg, obstacles_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg (:py:class:`~pylot.perception.messages.FrameMessage`): Message
received.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.ObstaclesMessage`
messages.
"""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self.config.name))
start_time = time.time()
# The models expect BGR images.
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
num_detections, res_boxes, res_scores, res_classes = self.__run_model(
msg.frame.frame)
obstacles = []
for i in range(0, num_detections):
if res_classes[i] in self._coco_labels:
if (res_scores[i] >=
self._flags.obstacle_detection_min_score_threshold):
if (self._coco_labels[res_classes[i]] in OBSTACLE_LABELS):
obstacles.append(
Obstacle(BoundingBox2D(
int(res_boxes[i][1] *
msg.frame.camera_setup.width),
int(res_boxes[i][3] *
msg.frame.camera_setup.width),
int(res_boxes[i][0] *
msg.frame.camera_setup.height),
int(res_boxes[i][2] *
msg.frame.camera_setup.height)),
res_scores[i],
self._coco_labels[res_classes[i]],
id=self._unique_id))
self._unique_id += 1
else:
self._logger.warning(
'Ignoring non essential detection {}'.format(
self._coco_labels[res_classes[i]]))
else:
self._logger.warning('Filtering unknown class: {}'.format(
res_classes[i]))
self._logger.debug('@{}: {} obstacles: {}'.format(
msg.timestamp, self.config.name, obstacles))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
# Send out obstacles.
obstacles_stream.send(
ObstaclesMessage(msg.timestamp, obstacles, runtime))
obstacles_stream.send(erdos.WatermarkMessage(msg.timestamp))
if self._flags.log_detector_output:
msg.frame.annotate_with_bounding_boxes(msg.timestamp, obstacles,
None, self._bbox_colors)
msg.frame.save(msg.timestamp.coordinates[0], self._flags.data_path,
'detector-{}'.format(self.config.name))
def on_msg_camera_stream(self, msg, obstacles_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg (:py:class:`~pylot.perception.messages.FrameMessage`): Message
received.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.ObstaclesMessage`
messages.
"""
operator_time_total_start = time.time()
start_time = time.time()
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self.config.name))
inputs = msg.frame.as_rgb_numpy_array()
results = []
start_time = time.time()
if MODIFIED_AUTOML:
(boxes_np, scores_np, classes_np,
num_detections_np) = self._tf_session.run(
self._detections_batch,
feed_dict={self._image_placeholder: inputs})
num_detections = num_detections_np[0]
boxes = boxes_np[0][:num_detections]
scores = scores_np[0][:num_detections]
classes = classes_np[0][:num_detections]
results = zip(boxes, scores, classes)
else:
outputs_np = self._tf_session.run(
self._detections_batch,
feed_dict={self._image_placeholder: inputs})[0]
for _, x, y, width, height, score, _class in outputs_np:
results.append(((y, x, y + height, x + width), score, _class))
obstacles = []
for (ymin, xmin, ymax, xmax), score, _class in results:
if np.isclose(ymin, ymax) or np.isclose(xmin, xmax):
continue
if MODIFIED_AUTOML:
# The alternate NMS implementation screws up the class labels.
_class = int(_class) + 1
if _class in self._coco_labels:
if (score >= self._flags.obstacle_detection_min_score_threshold
and self._coco_labels[_class]
in self._important_labels):
camera_setup = msg.frame.camera_setup
width, height = camera_setup.width, camera_setup.height
xmin, xmax = max(0, int(xmin)), min(int(xmax), width)
ymin, ymax = max(0, int(ymin)), min(int(ymax), height)
if xmin < xmax and ymin < ymax:
obstacles.append(
DetectedObstacle(BoundingBox2D(
xmin, xmax, ymin, ymax),
score,
self._coco_labels[_class],
id=self._unique_id))
self._unique_id += 1
self._csv_logger.info(
"{},{},detection,{},{:4f}".format(
pylot.utils.time_epoch_ms(), msg.timestamp,
self._coco_labels[_class], score))
else:
self._logger.debug(
'Filtering unknown class: {}'.format(_class))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp, obstacles,
None, self._bbox_colors)
if self._flags.visualize_detected_obstacles:
msg.frame.visualize(self.config.name,
pygame_display=pylot.utils.PYGAME_DISPLAY)
if self._flags.log_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'detector-{}'.format(self.config.name))
end_time = time.time()
obstacles_stream.send(ObstaclesMessage(msg.timestamp, obstacles, 0))
obstacles_stream.send(erdos.WatermarkMessage(msg.timestamp))
operator_time_total_end = time.time()
self._logger.debug("@{}: runtime of the detector: {}".format(
msg.timestamp, (end_time - start_time) * 1000))
self._logger.debug("@{}: total time spent: {}".format(
msg.timestamp,
(operator_time_total_end - operator_time_total_start) * 1000))
def on_watermark(self, timestamp, obstacles_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg (:py:class:`~pylot.perception.messages.FrameMessage`): Message
received.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.ObstaclesMessage`
messages.
"""
if timestamp.is_top:
return
start_time = time.time()
ttd_msg = self._ttd_msgs.popleft()
frame_msg = self._frame_msgs.popleft()
ttd = ttd_msg.data
self.update_model_choice(ttd)
frame = frame_msg.frame
inputs = frame.as_rgb_numpy_array()
detector_start_time = time.time()
outputs_np = self._tf_session.run(
self._signitures['prediction'],
feed_dict={self._signitures['image_arrays']: [inputs]})[0]
detector_end_time = time.time()
self._logger.debug("@{}: detector runtime {}".format(
timestamp, (detector_end_time - detector_start_time) * 1000))
obstacles = []
camera_setup = frame.camera_setup
for _, ymin, xmin, ymax, xmax, score, _class in outputs_np:
xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)
if _class in self._coco_labels:
if (score >= self._flags.obstacle_detection_min_score_threshold
and self._coco_labels[_class] in OBSTACLE_LABELS):
xmin, xmax = max(0, xmin), min(xmax, camera_setup.width)
ymin, ymax = max(0, ymin), min(ymax, camera_setup.height)
if xmin < xmax and ymin < ymax:
obstacles.append(
Obstacle(BoundingBox2D(xmin, xmax, ymin, ymax),
score,
self._coco_labels[_class],
id=self._unique_id))
self._unique_id += 1
self._csv_logger.info(
"{},{},detection,{},{:4f}".format(
pylot.utils.time_epoch_ms(),
timestamp.coordinates[0],
self._coco_labels[_class], score))
else:
self._logger.debug(
'Filtering unknown class: {}'.format(_class))
if self._flags.log_detector_output:
frame.annotate_with_bounding_boxes(timestamp, obstacles, None,
self._bbox_colors)
frame.save(timestamp.coordinates[0], self._flags.data_path,
'detector-{}'.format(self.config.name))
# end_time = time.time()
obstacles_stream.send(ObstaclesMessage(timestamp, obstacles, 0))
operator_time_total_end = time.time()
self._logger.debug("@{}: total time spent: {}".format(
timestamp, (operator_time_total_end - start_time) * 1000))
def on_msg_camera_stream(self, msg, obstacles_stream):
""" Invoked when the operator receives a message on the data stream."""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
# The models expect BGR images.
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(msg.frame.frame, axis=0)
(boxes, scores, classes, num_detections) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num_detections[0])
res_classes = classes[0][:num_detections]
res_boxes = boxes[0][:num_detections]
res_scores = scores[0][:num_detections]
obstacles = []
for i in range(0, num_detections):
if (res_classes[i] in self._coco_labels and res_scores[i] >=
self._flags.obstacle_detection_min_score_threshold):
obstacles.append(
DetectedObstacle(
BoundingBox2D(
int(res_boxes[i][1] *
msg.frame.camera_setup.width),
int(res_boxes[i][3] *
msg.frame.camera_setup.width),
int(res_boxes[i][0] *
msg.frame.camera_setup.height),
int(res_boxes[i][2] *
msg.frame.camera_setup.height)), res_scores[i],
self._coco_labels[res_classes[i]]))
else:
self._logger.warning('Filtering unknown class: {}'.format(
res_classes[i]))
self._logger.debug('@{}: {} obstacles: {}'.format(
msg.timestamp, self._name, obstacles))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp, obstacles,
self._bbox_colors)
if self._flags.visualize_detected_obstacles:
msg.frame.visualize(self._name)
if self._flags.log_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'detector-{}'.format(self._name))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
# Send out obstacles.
obstacles_stream.send(
ObstaclesMessage(obstacles, msg.timestamp, runtime))
def on_watermark(self, timestamp, obstacles_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg (:py:class:`~pylot.perception.messages.FrameMessage`): Message
received.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.ObstaclesMessage`
messages.
"""
start_time = time.time()
#ttd_msg = self._ttd_msgs.popleft()
frame_msg = self._frame_msgs.popleft()
#ttd, detection_deadline = ttd_msg.data
#self.update_model_choice(detection_deadline)
frame = frame_msg.frame
inputs = frame.as_rgb_numpy_array()
detector_start_time = time.time()
outputs_np = self._driver.serve_images([inputs])[0]
detector_end_time = time.time()
self._logger.debug("@{}: detector runtime {}".format(
timestamp, (detector_end_time - detector_start_time) * 1000))
obstacles = []
camera_setup = frame.camera_setup
for _, y, x, height, width, score, _class in outputs_np:
xmin = int(x)
ymin = int(y)
xmax = int(x + width)
ymax = int(y + height)
if _class in self._coco_labels:
if (score >= self._flags.obstacle_detection_min_score_threshold
and self._coco_labels[_class]
in self._important_labels):
xmin, xmax = max(0, xmin), min(xmax, camera_setup.width)
ymin, ymax = max(0, ymin), min(ymax, camera_setup.height)
if xmin < xmax and ymin < ymax:
obstacles.append(
DetectedObstacle(BoundingBox2D(
xmin, xmax, ymin, ymax),
score,
self._coco_labels[_class],
id=self._unique_id))
self._unique_id += 1
self._csv_logger.info(
"{},{},detection,{},{:4f}".format(
pylot.utils.time_epoch_ms(),
timestamp.coordinates[0],
self._coco_labels[_class], score))
else:
self._logger.debug(
'Filtering unknown class: {}'.format(_class))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
frame.annotate_with_bounding_boxes(timestamp, obstacles, None,
self._bbox_colors)
if self._flags.visualize_detected_obstacles:
frame.visualize(self.config.name,
pygame_display=pylot.utils.PYGAME_DISPLAY)
if self._flags.log_detector_output:
frame.save(timestamp.coordinates[0], self._flags.data_path,
'detector-{}'.format(self.config.name))
end_time = time.time()
obstacles_stream.send(ObstaclesMessage(timestamp, obstacles, 0))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
operator_time_total_end = time.time()
self._logger.debug("@{}: total time spent: {}".format(
timestamp, (operator_time_total_end - start_time) * 1000))
def on_msg_camera_stream(self, msg, obstacles_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg (:py:class:`~pylot.perception.messages.FrameMessage`): Message
received.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.ObstaclesMessage`
messages.
"""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self.config.name))
start_time = time.time()
# The models expect BGR images.
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(msg.frame.frame, axis=0)
(boxes, scores, classes, num_detections) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num_detections[0])
res_classes = [int(cls) for cls in classes[0][:num_detections]]
res_boxes = boxes[0][:num_detections]
res_scores = scores[0][:num_detections]
obstacles = []
for i in range(0, num_detections):
if res_classes[i] in self._coco_labels:
if (res_scores[i] >=
self._flags.obstacle_detection_min_score_threshold
and self._coco_labels[
res_classes[i]] in self._important_labels):
obstacles.append(
DetectedObstacle(BoundingBox2D(
int(res_boxes[i][1] *
msg.frame.camera_setup.width),
int(res_boxes[i][3] *
msg.frame.camera_setup.width),
int(res_boxes[i][0] *
msg.frame.camera_setup.height),
int(res_boxes[i][2] *
msg.frame.camera_setup.height)),
res_scores[i],
self._coco_labels[res_classes[i]],
id=self._unique_id))
self._unique_id += 1
else:
self._logger.warning('Filtering unknown class: {}'.format(
res_classes[i]))
self._logger.debug('@{}: {} obstacles: {}'.format(
msg.timestamp, self.config.name, obstacles))
if (self._flags.visualize_detected_obstacles
or self._flags.log_detector_output):
msg.frame.annotate_with_bounding_boxes(msg.timestamp, obstacles,
None, self._bbox_colors)
if self._flags.visualize_detected_obstacles:
msg.frame.visualize(self.config.name)
if self._flags.log_detector_output:
msg.frame.save(msg.timestamp.coordinates[0],
self._flags.data_path,
'detector-{}'.format(self.config.name))
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
# Send out obstacles.
obstacles_stream.send(
ObstaclesMessage(msg.timestamp, obstacles, runtime))
def on_watermark(self, timestamp, obstacles_error_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
assert len(timestamp.coordinates) == 1
op_start_time = time.time()
game_time = timestamp.coordinates[0]
if not self._last_notification:
self._last_notification = game_time
return
else:
self._sim_interval = (game_time - self._last_notification)
self._last_notification = game_time
sim_time = timestamp.coordinates[0]
while len(self._detector_start_end_times) > 0:
print("INSIDE EVAL WATERMARK", timestamp)
(end_time, start_time) = self._detector_start_end_times[0]
if end_time <= game_time:
heapq.heappop(self._detector_start_end_times)
ego_transform, ground_obstacles = self.__get_ground_obstacles_at(end_time)
#go_labels = []
#for go in ground_obstacles:
# go_labels.append(go.label)
obstacles = self.__get_obstacles_at(start_time)
#do_labels = []
#for do in obstacles:
# do_labels.append(do.label)
#print(timestamp, go_labels, do_labels)
if (len(obstacles) > 0 or len(ground_obstacles) > 0):
errs = pylot.perception.detection.utils.get_errors(
ground_obstacles, obstacles, ego_transform)
# Get runtime in ms
runtime = (time.time() - op_start_time) * 1000
# self._csv_logger.info('{},{},{},{},{:.4f}'.format(
# time_epoch_ms(), sim_time, self.config.name, 'runtime',
# runtime))
self._logger.info('errors calculated')
matchobs = []
for i in range(len(errs)):
ground_ob = errs[i][0]
det_ob = errs[i][1]
err_val = errs[i][2]
det_id = "NONE"
if (det_ob is not None):
det_ob.vis_error = err_val
det_id = det_ob.id
ego_point = ego_transform.location.as_numpy_array().reshape(1, 3)
ego_loc = ego_transform.inverse_transform_points(ego_point).reshape(3,)
ob_actual_point = ground_ob.transform.location.as_numpy_array().reshape(1, 3)
ob_actual_loc = ego_transform.inverse_transform_points(ob_actual_point).reshape(3,)
relative_dist = ob_actual_loc - ego_loc
#print([ground_ob.id, ego_loc, "===", ob_actual_loc, "====", relative_dist])
print([ground_ob.id, det_id, err_val])
self._csv_logger.info('{},{},{},{},{},{:.4f},{:.4f},{:.4f},{:.4f}'.format(
time_epoch_ms(), sim_time, self.config.name, ground_ob.id, ground_ob.label,
relative_dist[0], relative_dist[1], relative_dist[2],
err_val))
self._logger.debug('Computing accuracy for {} {}'.format(
end_time, start_time))
obstacles_error_stream.send(ObstaclesMessage(timestamp, obstacles))
obstacles_error_stream.send(erdos.WatermarkMessage(timestamp))
else:
# The remaining entries require newer ground obstacles.
break
self.__garbage_collect_obstacles()
def main(argv):
(obstacles_stream, traffic_lights_stream, obstacles_tracking_stream,
open_drive_stream, global_trajectory_stream, control_display_stream,
streams_to_send_top_on) = create_data_flow()
# Run the data-flow.
node_handle = erdos.run_async()
signal.signal(signal.SIGINT, shutdown)
# Send waypoints.
waypoints = Waypoints.read_from_csv_file(FLAGS.waypoints_csv_file,
FLAGS.target_speed)
global_trajectory_stream.send(
WaypointsMessage(
erdos.Timestamp(coordinates=[0]), waypoints,
pylot.planning.utils.BehaviorPlannerState.FOLLOW_WAYPOINTS))
# Send top watermark on all streams that require it.
top_msg = erdos.WatermarkMessage(erdos.Timestamp(is_top=True))
open_drive_stream.send(top_msg)
global_trajectory_stream.send(top_msg)
for stream in streams_to_send_top_on:
stream.send(top_msg)
time_to_sleep = 1.0 / FLAGS.sensor_frequency
count = 0
try:
while True:
timestamp = erdos.Timestamp(coordinates=[count])
if not FLAGS.obstacle_detection:
obstacles_stream.send(ObstaclesMessage(timestamp, []))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.traffic_light_detection:
traffic_lights_stream.send(TrafficLightsMessage(timestamp, []))
traffic_lights_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.obstacle_tracking:
obstacles_tracking_stream.send(
ObstacleTrajectoriesMessage(timestamp, []))
obstacles_tracking_stream.send(
erdos.WatermarkMessage(timestamp))
count += 1
if pylot.flags.must_visualize():
import pygame
from pygame.locals import K_n
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
# NOTE: We should offset sleep time by the time it takes to send
# the messages.
time.sleep(time_to_sleep)
except KeyboardInterrupt:
node_handle.shutdown()
if pylot.flags.must_visualize():
import pygame
pygame.quit()
