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 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, 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: Timestamp,
obstacles_stream: WriteStream):
"""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, []))
obstacles_stream.send(erdos.WatermarkMessage(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 = get_detected_speed_limits(
speed_limit_signs_msg.speed_signs, depth_msg.frame,
segmented_msg.frame)
det_stop_signs = 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, vehicle_transform))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if self._flags.log_detector_output:
bgr_msg.frame.annotate_with_bounding_boxes(bgr_msg.timestamp,
det_obstacles,
vehicle_transform)
bgr_msg.frame.save(bgr_msg.timestamp.coordinates[0],
self._flags.data_path, 'perfect-detector')
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 run_step(self, input_data, timestamp):
self._logger.debug("Current game time {}".format(timestamp))
erdos_timestamp = erdos.Timestamp(coordinates=[timestamp])
self.send_waypoints_msg(erdos_timestamp)
for key, val in input_data.items():
# print("{} {} {}".format(key, val[0], type(val[1])))
if key in self._camera_streams:
self._camera_streams[key].send(
pylot.perception.messages.FrameMessage(
CameraFrame(val[1], 'BGR', self._camera_setups[key]),
erdos_timestamp))
self._camera_streams[key].send(
erdos.WatermarkMessage(erdos_timestamp))
elif key == 'can_bus':
self.send_can_bus_msg(val[1], erdos_timestamp)
elif key == 'hdmap':
self.send_hd_map_msg(val[1], erdos_timestamp)
elif key == 'LIDAR':
self.send_lidar_msg(val[1], self._lidar_transform,
erdos_timestamp)
else:
self._logger.warning("Sensor {} not used".format(key))
# Wait until the control is set.
control_msg = self._control_stream.read()
output_control = carla.VehicleControl()
output_control.throttle = control_msg.throttle
output_control.brake = control_msg.brake
output_control.steer = control_msg.steer
output_control.reverse = control_msg.reverse
output_control.hand_brake = control_msg.hand_brake
output_control.manual_gear_shift = False
return output_control
def process_collision(self, collision_event):
""" Invoked when a collision event is received from the simulation.
Args:
collision_event (:py:class:`carla.CollisionEvent`): A collision
event that contains the impulse, location and the object with
which the ego-vehicle collided.
"""
game_time = int(collision_event.timestamp * 1000)
self._logger.debug(
"@[{}]: Received a collision event from the simulator.".format(
game_time))
# Create a CollisionMessage.
timestamp = erdos.Timestamp(coordinates=[game_time])
msg = CollisionMessage(
collision_event.other_actor,
Vector3D.from_simulator_vector(collision_event.normal_impulse),
timestamp)
# Send the CollisionMessage.
self._collision_stream.send(msg)
# TODO(ionel): This code will fail if process_collision is called twice
# for the same timestamp (i.e., if the vehicle collides with two other
# actors)
self._collision_stream.send(erdos.WatermarkMessage(timestamp))
def run(self):
# Connect to CARLA and retrieve the world running.
self._client, self._world = get_world(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
if self._client is None or self._world is None:
raise ValueError('There was an issue connecting to the simulator.')
# Replayer time factor is only available in > 0.9.5.
# self._client.set_replayer_time_factor(0.1)
print(
self._client.replay_file(self._flags.carla_replay_file,
self._flags.carla_replay_start_time,
self._flags.carla_replay_duration,
self._flags.carla_replay_id))
# Sleep a bit to allow the server to start the replay.
time.sleep(1)
self._driving_vehicle = self._world.get_actors().find(
self._flags.carla_replay_id)
if self._driving_vehicle is None:
raise ValueError("There was an issue finding the vehicle.")
timestamp = erdos.Timestamp(is_top=True)
vehicle_id_msg = erdos.Message(timestamp, self._driving_vehicle.id)
self._vehicle_id_stream.send(vehicle_id_msg)
self._vehicle_id_stream.send(erdos.WatermarkMessage(timestamp))
self._world.on_tick(self.on_world_tick)
def send_lidar_msg(self,
point_cloud_stream,
simulator_pc,
timestamp,
lidar_setup,
ego_transform=None):
"""Transforms and sends a point cloud reading.
This method is transforms a point cloud from the challenge format
to the type Pylot uses, and it sends it on the point cloud stream.
"""
# Remove the intensity component of the point cloud.
simulator_pc = simulator_pc[:, :3]
point_cloud = PointCloud(simulator_pc, lidar_setup)
if self._last_point_cloud is not None:
# TODO(ionel): Should offset the last point cloud wrt to the
# current location.
# self._last_point_cloud.global_points = \
# ego_transform.transform_points(
# self._last_point_cloud.global_points)
# self._last_point_cloud.points = \
# self._last_point_cloud._to_camera_coordinates(
# self._last_point_cloud.global_points)
point_cloud.merge(self._last_point_cloud)
point_cloud_stream.send(
pylot.perception.messages.PointCloudMessage(
timestamp, point_cloud))
point_cloud_stream.send(erdos.WatermarkMessage(timestamp))
# global_pc = ego_transform.inverse_transform_points(simulator_pc)
self._last_point_cloud = PointCloud(simulator_pc, lidar_setup)
def process_imu(self, imu_msg):
"""Invoked when an IMU message is received from the simulator.
Sends IMU measurements to downstream operators.
Args:
imu_msg (carla.IMUMeasurement): IMU reading.
"""
game_time = int(imu_msg.timestamp * 1000)
timestamp = erdos.Timestamp(coordinates=[game_time])
watermark_msg = erdos.WatermarkMessage(timestamp)
with erdos.profile(self.config.name + '.process_imu',
self,
event_data={'timestamp': str(timestamp)}):
with self._lock:
msg = IMUMessage(
timestamp,
Transform.from_carla_transform(imu_msg.transform),
Vector3D.from_carla_vector(imu_msg.accelerometer),
Vector3D.from_carla_vector(imu_msg.gyroscope),
imu_msg.compass)
self._imu_stream.send(msg)
# Note: The operator is set not to automatically propagate
# watermark messages received on input streams. Thus, we can
# issue watermarks only after the Carla callback is invoked.
self._imu_stream.send(watermark_msg)
def process_point_clouds(self, simulator_pc):
""" Invoked when a point cloud is received from the simulator.
"""
game_time = int(simulator_pc.timestamp * 1000)
timestamp = erdos.Timestamp(coordinates=[game_time])
watermark_msg = erdos.WatermarkMessage(timestamp)
with erdos.profile(self.config.name + '.process_point_clouds',
self,
event_data={'timestamp': str(timestamp)}):
# Ensure that the code executes serially
with self._lock:
assert len(
simulator_pc.raw_data) > 0, 'Lidar did not send any points'
# Include the transform relative to the vehicle.
# simulator_pc.transform returns the world transform, but
# we do not use it directly.
msg = PointCloudMessage(
timestamp,
PointCloud.from_simulator_point_cloud(
simulator_pc, self._lidar_setup))
if self._release_data:
self._lidar_stream.send(msg)
self._lidar_stream.send(watermark_msg)
else:
# Pickle the data, and release it upon release msg receipt.
pickled_msg = pickle.dumps(
msg, protocol=pickle.HIGHEST_PROTOCOL)
with self._pickle_lock:
self._pickled_messages[msg.timestamp] = pickled_msg
self._notify_reading_stream.send(watermark_msg)
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 send_pose_msg(self, speed_data, imu_data, gnss_data, timestamp):
forward_speed = speed_data['speed']
# vehicle_transform = pylot.utils.Transform.from_carla_transform(
# speed_data['transform'])
latitude = gnss_data[0]
longitude = gnss_data[1]
altitude = gnss_data[2]
location = pylot.utils.Location.from_gps(latitude, longitude, altitude)
if np.isnan(imu_data[6]):
yaw = self._last_yaw
else:
compass = np.degrees(imu_data[6])
if compass < 270:
yaw = compass - 90
else:
yaw = compass - 450
self._last_yaw = yaw
vehicle_transform = pylot.utils.Transform(
location, pylot.utils.Rotation(yaw=yaw))
velocity_vector = pylot.utils.Vector3D(forward_speed * np.cos(yaw),
forward_speed * np.sin(yaw), 0)
current_pose = pylot.utils.Pose(vehicle_transform, forward_speed,
velocity_vector,
timestamp.coordinates[0])
self._logger.debug("@{}: Predicted pose: {}".format(
timestamp, current_pose))
self._pose_stream.send(erdos.Message(timestamp, current_pose))
self._pose_stream.send(erdos.WatermarkMessage(timestamp))
def on_frame(self, msg, traffic_lights_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg: A :py:class:`~pylot.perception.messages.FrameMessage`.
obstacles_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator sends
:py:class:`~pylot.perception.messages.TrafficLightsMessage`
messages for traffic lights.
"""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self.config.name))
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
boxes, scores, labels = self.__run_model(
msg.frame.as_rgb_numpy_array())
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.config.name, traffic_lights))
traffic_lights_stream.send(
TrafficLightsMessage(msg.timestamp, traffic_lights))
traffic_lights_stream.send(erdos.WatermarkMessage(msg.timestamp))
if self._flags.log_traffic_light_detector_output:
msg.frame.annotate_with_bounding_boxes(msg.timestamp,
traffic_lights)
msg.frame.save(msg.timestamp.coordinates[0], self._flags.data_path,
'tl-detector-{}'.format(self.config.name))
def on_sensor_ready(self, timestamp, release_sensor_stream):
""" Invoked upon receipt of a notification of the sensors being
ready for the given timestamp.
Releases a watermark on the release_sensor_stream to notify all the
sensors to release their data for the given timestamp.
Args:
timestamp (:py:class:`erdos.Timestamp`): The timestamp of the
watermark.
sensor_ready_stream (:py:class:`erdos.WriteStream`): The stream
on which to write the notification.
"""
self._logger.debug("@{}: the sensors are all ready.".format(timestamp))
release_sensor_stream.send(erdos.WatermarkMessage(timestamp))
# Retrieve the game time.
game_time = timestamp.coordinates[0]
# Also rewrite the receive time for the pose update because the sensor
# callbacks might take too long.
if game_time in self._pose_map:
self._pose_map[game_time][1] = time.time()
else:
self._pose_map[game_time] = [None, time.time()]
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
self.update_world(timestamp)
ttd_msg = self._ttd_msgs.popleft()
# Total ttd - time spent up to now
ttd = ttd_msg.data - (time.time() - self._world.pose.localization_time)
self._logger.debug('@{}: adjusting ttd from {} to {}'.format(
timestamp, ttd_msg.data, ttd))
# if self._state == BehaviorPlannerState.OVERTAKE:
# # Ignore traffic lights and obstacle.
# output_wps = self._planner.run(timestamp, ttd)
# else:
(speed_factor, _, _, speed_factor_tl,
speed_factor_stop) = self._world.stop_for_agents(timestamp)
if self._flags.planning_type == 'waypoint':
target_speed = speed_factor * self._flags.target_speed
self._logger.debug(
'@{}: speed factor: {}, target speed: {}'.format(
timestamp, speed_factor, target_speed))
output_wps = self._world.follow_waypoints(target_speed)
else:
output_wps = self._planner.run(timestamp, ttd)
speed_factor = min(speed_factor_stop, speed_factor_tl)
self._logger.debug('@{}: speed factor: {}'.format(
timestamp, speed_factor))
output_wps.apply_speed_factor(speed_factor)
waypoints_stream.send(WaypointsMessage(timestamp, output_wps))
waypoints_stream.send(erdos.WatermarkMessage(timestamp))
def run_step(self, input_data, timestamp):
game_time = int(timestamp * 1000)
self._logger.debug("Current game time {}".format(game_time))
erdos_timestamp = erdos.Timestamp(coordinates=[game_time])
if not self._sent_open_drive and \
self.track != Track.ALL_SENSORS_HDMAP_WAYPOINTS:
# We do not have access to the open drive map. Send top watermark.
self._sent_open_drive = True
self._open_drive_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
self.send_waypoints_msg(erdos_timestamp)
for key, val in input_data.items():
# print("{} {} {}".format(key, val[0], type(val[1])))
if key in self._camera_streams:
self._camera_streams[key].send(
pylot.perception.messages.FrameMessage(
erdos_timestamp,
CameraFrame(val[1][:, :, :3], 'BGR',
self._camera_setups[key])))
self._camera_streams[key].send(
erdos.WatermarkMessage(erdos_timestamp))
elif key == 'can_bus':
self.send_pose_msg(val[1], erdos_timestamp)
elif key == 'hdmap':
self.send_hd_map_msg(val[1], erdos_timestamp)
elif key == 'LIDAR':
self.send_lidar_msg(val[1], self._lidar_transform,
erdos_timestamp)
else:
self._logger.warning("Sensor {} not used".format(key))
# Wait until the control is set.
while True:
control_msg = self._control_stream.read()
if not isinstance(control_msg, erdos.WatermarkMessage):
output_control = carla.VehicleControl()
output_control.throttle = control_msg.throttle
output_control.brake = control_msg.brake
output_control.steer = control_msg.steer
output_control.reverse = control_msg.reverse
output_control.hand_brake = control_msg.hand_brake
output_control.manual_gear_shift = False
return output_control
def on_pose_watermark(self, timestamp, waypoint_stream, pose_stream):
""" Invoked upon receipt of the watermark on the pose stream.
This callback matches the waypoints to the given timestamp and releases
both the waypoints and the pose message to the control operator.
Args:
timestamp (:py:class:`erdos.Timestamp`): The timestamp of the
watermark.
waypoint_stream (:py:class:`erdos.WriteStream`): The stream to send
the waypoints out on.
pose_stream (:py:class:`erdos.WriteStream`): The stream to send
the pose out on.
"""
self._logger.info("@{}: received pose watermark.".format(timestamp))
# Retrieve the game time.
game_time = timestamp.coordinates[0]
# Retrieve the pose message for the given timestamp.
pose_msg, pose_ingress_time = self._pose_map[game_time]
# Match the waypoints to the given timestamp.
waypoint_index, waypoints = -1, None
for i, (time, _waypoints) in enumerate(self._waypoints):
if time <= game_time:
waypoint_index, waypoints = i, _waypoints
else:
break
self._logger.debug("@{} waypoint index is {}".format(
timestamp, waypoint_index))
if waypoints is None:
# If we haven't received a single waypoint, send an empty message.
self._waypoints_write_stream.send(
WaypointsMessage(timestamp, deque([]), deque([])))
else:
# Send the trimmed waypoints on the write stream.
trimmed_waypoints, trimmed_target_speeds = \
remove_completed_waypoints(
deepcopy(waypoints.waypoints),
deepcopy(waypoints.target_speeds),
pose_msg.data.transform.location)
waypoints_msg = WaypointsMessage(timestamp, trimmed_waypoints,
trimmed_target_speeds)
self._waypoints_write_stream.send(waypoints_msg)
# Trim the saved waypoints.
for i in range(waypoint_index):
self._logger.debug("@{}: Pruning {}".format(
timestamp, self._waypoints.popleft()))
# Send the pose and the watermark messages.
watermark = erdos.WatermarkMessage(timestamp)
pose_stream.send(pose_msg)
pose_stream.send(watermark)
waypoint_stream.send(watermark)
# Clean up the pose from the dict.
del self._pose_map[game_time]
def send_imu_msg(self, imu_data, timestamp):
accelerometer = pylot.utils.Vector3D(imu_data[0], imu_data[1],
imu_data[2])
gyroscope = pylot.utils.Vector3D(imu_data[3], imu_data[4], imu_data[5])
compass = imu_data[6]
msg = IMUMessage(timestamp, None, accelerometer, gyroscope, compass)
self._imu_stream.send(msg)
self._imu_stream.send(erdos.WatermarkMessage(timestamp))
def on_pose_update(self, msg, time_to_decision_stream):
self._logger.debug('@{}: {} received pose message'.format(
msg.timestamp, self.config.name))
ttd = TimeToDecisionOperator.time_to_decision(msg.data.transform,
msg.data.forward_speed,
None)
time_to_decision_stream.send(erdos.Message(msg.timestamp, ttd))
time_to_decision_stream.send(erdos.WatermarkMessage(msg.timestamp))
def send_gnss_msg(self, gnss_data, timestamp):
latitude = gnss_data[0]
longitude = gnss_data[1]
altitude = gnss_data[2]
location = pylot.utils.Location.from_gps(latitude, longitude, altitude)
transform = pylot.utils.Transform(location, pylot.utils.Rotation())
msg = GNSSMessage(timestamp, transform, altitude, latitude, longitude)
self._gnss_stream.send(msg)
self._gnss_stream.send(erdos.WatermarkMessage(timestamp))
def send_hd_map_msg(self, data, timestamp):
# Sending once opendrive data
if self._open_drive_data is None:
self._open_drive_data = data['opendrive']
self._open_drive_stream.send(
erdos.Message(timestamp, self._open_drive_data))
self._open_drive_stream.send(
erdos.WatermarkMessage(
erdos.Timestamp(coordinates=[sys.maxsize])))
def __send_world_data(self):
""" Sends ego vehicle id, open drive and trajectory messages."""
# Send the id of the ego vehicle. This id is used by the driver
# operators to get a handle to the ego vehicle, which they use to
# attach sensors.
self.vehicle_id_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
self._ego_vehicle.id))
self.vehicle_id_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
# Send open drive string.
self.open_drive_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
self._world.get_map().to_opendrive()))
top_watermark = erdos.WatermarkMessage(erdos.Timestamp(is_top=True))
self.open_drive_stream.send(top_watermark)
self.global_trajectory_stream.send(top_watermark)
def setup(self, path_to_conf_file):
"""Setup phase. Invoked by the scenario runner."""
# Disable Tensorflow logging.
pylot.utils.set_tf_loglevel(logging.ERROR)
# Parse the flag file.
flags.FLAGS([__file__, '--flagfile={}'.format(path_to_conf_file)])
self._logger = erdos.utils.setup_logging('erdos_agent',
FLAGS.log_file_name)
enable_logging()
self.track = get_track()
self._town_name = None
self._camera_setups = create_camera_setups()
# Set the lidar in the same position as the center camera.
self._lidar_transform = pylot.utils.Transform(CENTER_CAMERA_LOCATION,
pylot.utils.Rotation())
self._lidar_setup = LidarSetup('lidar',
'sensor.lidar.ray_cast',
self._lidar_transform,
range=8500,
legacy=False)
self._last_point_cloud = None
self._last_yaw = 0
# Stores the waypoints we get from the challenge planner.
self._waypoints = None
# Stores the open drive string we get when we run in track 3.
self._open_drive_data = None
(camera_streams, pose_stream, route_stream, global_trajectory_stream,
open_drive_stream, point_cloud_stream, imu_stream, gnss_stream,
control_stream, control_display_stream, ground_obstacles_stream,
ground_traffic_lights_stream, vehicle_id_stream,
streams_to_send_top_on) = create_data_flow()
self._camera_streams = camera_streams
self._pose_stream = pose_stream
self._route_stream = route_stream
self._sent_initial_pose = False
self._global_trajectory_stream = global_trajectory_stream
self._open_drive_stream = open_drive_stream
self._sent_open_drive = False
self._point_cloud_stream = point_cloud_stream
self._imu_stream = imu_stream
self._gnss_stream = gnss_stream
self._control_stream = control_stream
self._control_display_stream = control_display_stream
self._ground_obstacles_stream = ground_obstacles_stream
self._ground_traffic_lights_stream = ground_traffic_lights_stream
self._vehicle_id_stream = vehicle_id_stream
self._ego_vehicle = None
self._sent_vehicle_id = False
# Execute the dataflow.
self._node_handle = erdos.run_async()
for stream in streams_to_send_top_on:
stream.send(erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
if using_perfect_component():
# The agent is using a perfect component. It must directly connect
# to the simulator to send perfect data to the data-flow.
_, self._world = pylot.simulation.utils.get_world(
FLAGS.carla_host, FLAGS.carla_port, FLAGS.carla_timeout)
def on_position_update(self, timestamp: Timestamp,
detected_lane_stream: WriteStream):
"""Invoked on the receipt of an update to the position of the vehicle.
Uses the position of the vehicle to get future waypoints and draw
lane markings using those waypoints.
Args:
pose_msg: Contains the current location of the ego vehicle.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
bgr_msg = self._bgr_msgs.popleft()
pose_msg = self._pose_msgs.popleft()
vehicle_location = pose_msg.data.transform.location
self._frame_cnt += 1
if self._map:
lanes = self._map.get_all_lanes(vehicle_location)
if self._flags.log_lane_detection_camera:
camera_setup = bgr_msg.frame.camera_setup
frame = np.zeros((camera_setup.height, camera_setup.width),
dtype=np.dtype("uint8"))
binary_frame = frame.copy()
for lane in lanes:
lane.collect_frame_data(frame,
binary_frame,
camera_setup,
inverse_transform=pose_msg.data.
transform.inverse_transform())
self._logger.debug('@{}: detected {} lanes'.format(
bgr_msg.timestamp, len(lanes)))
if self._frame_cnt % self._flags.log_every_nth_message == 0:
instance_file_name = os.path.join(
self._flags.data_path,
'{}-{}.png'.format("lane-",
bgr_msg.timestamp.coordinates[0]))
binary_file_name = os.path.join(
self._flags.data_path,
'{}-{}.png'.format("binary_lane-",
bgr_msg.timestamp.coordinates[0]))
instance_img = Image.fromarray(frame)
binary_img = Image.fromarray(binary_frame)
instance_img.save(instance_file_name)
binary_img.save(binary_file_name)
self._logger_lane.debug(
'@{}: Created binary lane and lane images in {}'.
format(pose_msg.timestamp, self._flags.data_path))
else:
for lane in lanes:
lane.draw_on_world(self._world)
else:
self._logger.debug('@{}: map is not ready yet'.format(
pose_msg.timestamp))
lanes = []
output_msg = LanesMessage(pose_msg.timestamp, lanes)
detected_lane_stream.send(output_msg)
detected_lane_stream.send(erdos.WatermarkMessage(pose_msg.timestamp))
def _send_world_messages(self):
""" Sends initial open drive and trajectory messages."""
# Send open drive string.
top_timestamp = erdos.Timestamp(coordinates=[sys.maxsize])
self.open_drive_stream.send(
erdos.Message(top_timestamp,
self._world.get_map().to_opendrive()))
top_watermark = erdos.WatermarkMessage(top_timestamp)
self.open_drive_stream.send(top_watermark)
self.global_trajectory_stream.send(top_watermark)
def send_perfect_pose_msg(self, timestamp):
vec_transform = pylot.utils.Transform.from_carla_transform(
self._ego_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_carla_vector(
self._ego_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector,
timestamp.coordinates[0])
self._pose_stream.send(erdos.Message(timestamp, pose))
self._pose_stream.send(erdos.WatermarkMessage(timestamp))
def send_initial_pose_msg(self, timestamp):
if not self._sent_initial_pose:
self._sent_initial_pose = True
initial_transform = self._global_plan_world_coord[0][0]
initial_pose = pylot.utils.Pose(
pylot.utils.Transform.from_carla_transform(initial_transform),
0, pylot.utils.Vector3D(), timestamp.coordinates[0])
self._route_stream.send(erdos.Message(timestamp, initial_pose))
self._route_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def __send_hero_vehicle_data(self, stream, timestamp, watermark_msg):
vec_transform = pylot.utils.Transform.from_carla_transform(
self._ego_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_carla_vector(
self._ego_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
print("{} Forward speed is {}".format(timestamp, forward_speed))
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector)
stream.send(erdos.Message(timestamp, pose))
stream.send(erdos.WatermarkMessage(timestamp))
def send_imu_msg(self, imu_stream, imu_data, timestamp):
"""Sends the IMU data on the Pylot stream."""
accelerometer = pylot.utils.Vector3D(imu_data[0], imu_data[1],
imu_data[2])
gyroscope = pylot.utils.Vector3D(imu_data[3], imu_data[4], imu_data[5])
compass = imu_data[6]
# Build a Pylot IMUMessage out of the challenge IMU sensor data.
msg = IMUMessage(timestamp, None, accelerometer, gyroscope, compass)
imu_stream.send(msg)
imu_stream.send(erdos.WatermarkMessage(timestamp))
