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))