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 on_watermark(self, timestamp, tracked_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))
if timestamp.is_top:
tracked_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
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)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
# Ignore obstacles that are far away.
if (vehicle_transform.location.distance(
obstacle.transform.location) >
self._flags.dynamic_obstacle_distance_threshold):
continue
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
# The trajectory is relative to the current location.
obstacle_trajectories.append(
ObstacleTrajectory(obstacle, cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
tracked_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
self._log_obstacles(timestamp, obstacles_with_location)
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
def on_watermark(self, timestamp, tracked_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))
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)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
for obstacle in obstacles_with_location:
obstacle_location = obstacle.transform.location
x = obstacle_location.x
y = obstacle_location.y
z = obstacle_location.z
self._csv_logger.debug('{},{},obstacle,{},{}'.format(
pylot.utils.time_epoch_ms(), timestamp.coordinates[0],
"[{} {}]".format(obstacle.id, obstacle.label),
"[{:.4f} {:.4f} {:.4f}]".format(x, y, z)))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
def on_watermark(self, timestamp, tracked_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))
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)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
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)
def on_watermark(self, timestamp, tracked_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))
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._can_bus_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)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
cur_obstacle_trajectory = [
obstacle.transform
for obstacle in self._obstacle_history[obstacle.id]
]
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
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)
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))