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 on_watermark(self, timestamp, trajectory_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
pose_msg = self._pose_msgs.popleft()
ego_transform = pose_msg.data.transform
self._ego_info.update(self._state, pose_msg)
old_state = self._state
self._state = self.__best_state_transition(self._ego_info)
self._logger.debug('@{}: agent transitioned from {} to {}'.format(
timestamp, old_state, self._state))
# Remove the waypoint from the route if we're close to it.
if (old_state != self._state
and self._state == BehaviorPlannerState.OVERTAKE):
self._route.remove_waypoint_if_close(ego_transform.location, 10)
else:
if not self._map.is_intersection(ego_transform.location):
self._route.remove_waypoint_if_close(ego_transform.location, 5)
else:
self._route.remove_waypoint_if_close(ego_transform.location,
3.5)
new_goal_location = None
if len(self._route.waypoints) > 1:
new_goal_location = self._route.waypoints[1].location
elif len(self._route.waypoints) == 1:
new_goal_location = self._route.waypoints[0].location
else:
new_goal_location = ego_transform.location
if new_goal_location != self._goal_location:
self._goal_location = new_goal_location
if self._map:
# Use the map to compute more fine-grained waypoints.
waypoints = self._map.compute_waypoints(
ego_transform.location, self._goal_location)
road_options = deque([
pylot.utils.RoadOption.LANE_FOLLOW
for _ in range(len(waypoints))
])
waypoints = Waypoints(waypoints, road_options=road_options)
else:
# Map is not available, send the route.
waypoints = self._route
if not waypoints or waypoints.is_empty():
# If waypoints are empty (e.g., reached destination), set
# waypoints to current vehicle location.
waypoints = Waypoints(
deque([ego_transform]),
road_options=deque([pylot.utils.RoadOption.LANE_FOLLOW]))
trajectory_stream.send(
WaypointsMessage(timestamp, waypoints, self._state))
elif old_state != self._state:
# Send the state update.
trajectory_stream.send(
WaypointsMessage(timestamp, None, self._state))
trajectory_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp: erdos.Timestamp,
waypoints_stream: erdos.WriteStream):
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
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))
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: RRT* succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the optimal frenet path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.debug("@{}: Frenet Optimal Trajectory failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: Frenet Optimal Trajectory succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def on_can_bus_update(self, msg, waypoints_stream):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._vehicle_transform = msg.data.transform
self.__update_waypoints()
next_waypoint_steer = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_steer)]
next_waypoint_speed = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_speed)]
# Get vectors and angles to corresponding speed and steer waypoints.
wp_steer_vector, wp_steer_angle = get_waypoint_vector_and_angle(
next_waypoint_steer, self._vehicle_transform)
wp_speed_vector, wp_speed_angle = get_waypoint_vector_and_angle(
next_waypoint_speed, self._vehicle_transform)
head_waypoints = collections.deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
output_msg = WaypointsMessage(msg.timestamp,
waypoints=head_waypoints,
target_speed=self._flags.target_speed,
wp_angle=wp_steer_angle,
wp_vector=wp_steer_vector,
wp_angle_speed=wp_speed_angle)
waypoints_stream.send(output_msg)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
else:
# haven't received waypoints from global trajectory stream
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# run rrt star
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
success, (path_x, path_y) = \
self._apply_rrt_star(obstacle_list, timestamp)
speeds = [0]
if success:
speeds = [self._flags.target_speed] * len(path_x)
self._logger.debug("@{}: RRT* Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: RRT* Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: RRT* Speeds: {}".format(
timestamp,
[self._flags.target_speed] * len(path_x))
)
# construct and send waypoint message
waypoint_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoint_message)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
# get obstacles
obstacle_map = self._build_obstacle_map(vehicle_transform)
# compute goals
target_location = self._compute_target_location(vehicle_transform)
# run rrt*
path, cost = self._run_rrt_star(vehicle_transform, target_location,
obstacle_map)
# convert to waypoints if path found, else use default waypoints
if cost is not None:
path_transforms = []
for point in path:
p_loc = self._carla_map.get_waypoint(
carla.Location(x=point[0], y=point[1], z=0),
project_to_road=True,
).transform.location # project to road to approximate Z
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
waypoints = deque(path_transforms)
waypoints.extend(
itertools.islice(self._waypoints, self._wp_index,
len(self._waypoints))
) # add the remaining global route for future
else:
waypoints = self._waypoints
# construct waypoints message
waypoints = collections.deque(
itertools.islice(waypoints, 0,
DEFAULT_NUM_WAYPOINTS)) # only take 50 meters
next_waypoint = waypoints[self._wp_index]
wp_steer_speed_vector, wp_steer_speed_angle = \
get_waypoint_vector_and_angle(
next_waypoint, vehicle_transform
)
output_msg = WaypointsMessage(timestamp,
waypoints=waypoints,
wp_angle=wp_steer_speed_angle,
wp_vector=wp_steer_speed_vector,
wp_angle_speed=wp_steer_speed_angle)
# send waypoints message
waypoints_stream.send(output_msg)
waypoints_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
# haven't received waypoints from global trajectory stream
else:
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# compute optimal frenet trajectory
path_x, path_y, speeds, params, success, s0 = \
self._compute_optimal_frenet_trajectory(can_bus_msg, obstacle_list)
if success:
self._logger.debug("@{}: Frenet Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: Frenet Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: Frenet Speeds: {}".format(
timestamp,
speeds.tolist())
)
# construct and send waypoint message
waypoints_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoints_message)
def on_can_bus_update(self, msg):
start_time = time.time()
(wp_angle, wp_vector, wp_angle_speed, wp_vector_speed) = self.get_waypoints(msg.data.transform)
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},{}'.format(
time_epoch_ms(), self.name, runtime))
output_msg = WaypointsMessage(
msg.timestamp,
wp_angle=wp_angle,
wp_vector=wp_vector,
wp_angle_speed=wp_angle_speed,
wp_vector_speed=wp_vector_speed)
self.get_output_stream('waypoints').send(output_msg)
def send_waypoints_msg(self, timestamp):
# Send once the global waypoints.
if self._waypoints is None:
# Gets global waypoints from the agent.
waypoints = deque([])
road_options = deque([])
for (transform, road_option) in self._global_plan_world_coord:
waypoints.append(
pylot.utils.Transform.from_carla_transform(transform))
road_options.append(pylot.utils.RoadOption(road_option.value))
self._waypoints = Waypoints(waypoints, road_options=road_options)
self._global_trajectory_stream.send(
WaypointsMessage(timestamp, self._waypoints))
self._global_trajectory_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def send_global_trajectory_msg(self, global_trajectory_stream, timestamp):
"""Sends the route the agent must follow."""
# Send once the global waypoints.
if not global_trajectory_stream.is_closed():
# Gets global waypoints from the agent.
waypoints = deque([])
road_options = deque([])
for (transform, road_option) in self._global_plan_world_coord:
waypoints.append(
pylot.utils.Transform.from_simulator_transform(transform))
road_options.append(pylot.utils.RoadOption(road_option.value))
waypoints = Waypoints(waypoints, road_options=road_options)
global_trajectory_stream.send(
WaypointsMessage(timestamp, waypoints))
global_trajectory_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def _construct_waypoints(self, timestamp, pose_msg, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = deque()
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
x = pose_msg.data.transform.location.x
y = pose_msg.data.transform.location.y
current_index = 0
min_dist = np.infty
for i, wp in enumerate(self._prev_waypoints):
dist = np.linalg.norm([wp.location.x - x, wp.location.y - y])
if dist <= min_dist:
current_index = i
min_dist = dist
for wp in itertools.islice(
self._prev_waypoints, current_index,
current_index + self._flags.num_waypoints_ahead):
path_transforms.append(wp)
target_speeds.append(0)
waypoints = deque(path_transforms)
else:
self._logger.debug("@{}: RRT* succeeded.".format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region it
# constructs search space as a top down, minimum bounding
# rectangle with padding in each dimension.
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
self._watermark_cnt += 1
# Get hero vehicle info.
pose_msg = self._pose_msgs.popleft()
self._vehicle_transform = pose_msg.data.transform
tl_msg = self._traffic_light_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
if (self._recompute_waypoints
and self._watermark_cnt % RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS
== 0):
self._waypoints = self._map.compute_waypoints(
self._vehicle_transform.location, self._goal_location)
self._waypoints, _ = remove_completed_waypoints(
self._waypoints, None, self._vehicle_transform.location,
WAYPOINT_COMPLETION_THRESHOLD)
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = deque([self._vehicle_transform])
wp_vector, wp_angle = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
self._vehicle_transform, self._waypoints,
DEFAULT_TARGET_WAYPOINT)
speed_factor, _ = pylot.planning.utils.stop_for_agents(
self._vehicle_transform.location, wp_angle, wp_vector,
obstacles_msg.obstacles, tl_msg.obstacles, self._flags,
self._logger, self._map, timestamp)
target_speed = speed_factor * self._flags.target_speed
self._logger.debug('@{}: computed speed factor: {}'.format(
timestamp, speed_factor))
self._logger.debug('@{}: computed target speed: {}'.format(
timestamp, target_speed))
head_waypoints = deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
target_speeds = deque(
[target_speed for _ in range(len(head_waypoints))])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
def _construct_waypoints(self, timestamp, pose_msg, path_x, path_y, speeds,
success):
"""
Convert the optimal frenet path into a waypoints message.
"""
path_transforms = []
target_speeds = deque()
if not success:
self._logger.debug("@{}: Frenet Optimal Trajectory failed. "
"Sending emergency stop.".format(timestamp))
x = pose_msg.data.transform.location.x
y = pose_msg.data.transform.location.y
current_index = 0
min_dist = np.infty
for i, wp in enumerate(self._prev_waypoints):
dist = np.linalg.norm([wp.location.x - x, wp.location.y - y])
if dist <= min_dist:
current_index = i
min_dist = dist
for wp in itertools.islice(
self._prev_waypoints, current_index,
current_index + self._flags.num_waypoints_ahead):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug(
"@{}: Frenet Optimal Trajectory succeeded.".format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def on_can_bus_update(self, msg):
self._vehicle_transform = msg.data.transform
(next_waypoint_steer, next_waypoint_speed) = self.__update_waypoints()
# Get vectors and angles to corresponding speed and steer waypoints.
wp_steer_vector, wp_steer_angle = get_waypoint_vector_and_angle(
next_waypoint_steer, self._vehicle_transform)
wp_speed_vector, wp_speed_angle = get_waypoint_vector_and_angle(
next_waypoint_speed, self._vehicle_transform)
target_speed = get_target_speed(self._vehicle_transform.location,
next_waypoint_steer)
output_msg = WaypointsMessage(msg.timestamp,
waypoints=[next_waypoint_steer],
target_speed=target_speed,
wp_angle=wp_steer_angle,
wp_vector=wp_steer_vector,
wp_angle_speed=wp_speed_angle)
self.get_output_stream('waypoints').send(output_msg)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
pose_msg = self._pose_msgs.popleft()
vehicle_transform = pose_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self.build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
self._prev_waypoints = self._waypoints
# haven't received waypoints from global trajectory stream
else:
self._logger.debug(
"@{}: Sending target speed 0, haven't"
"received global trajectory".format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# compute optimal frenet trajectory
initial_conditions = self._compute_initial_conditions(
pose_msg, obstacle_list)
start = time.time()
(path_x, path_y, speeds, ix, iy, iyaw, d, s, speeds_x, speeds_y, misc,
costs, success) = run_fot(initial_conditions, self._hyperparameters)
fot_runtime = (time.time() - start) * 1000
self._logger.debug('@{}: Frenet runtime {}'.format(
timestamp, fot_runtime))
if success:
self._logger.debug("@{}: Frenet Path X: {}".format(
timestamp, path_x.tolist()))
self._logger.debug("@{}: Frenet Path Y: {}".format(
timestamp, path_y.tolist()))
self._logger.debug("@{}: Frenet Speeds: {}".format(
timestamp, speeds.tolist()))
self._logger.debug("@{}: Frenet IX: {}".format(
timestamp, ix.tolist()))
self._logger.debug("@{}: Frenet IY: {}".format(
timestamp, iy.tolist()))
self._logger.debug("@{}: Frenet IYAW: {}".format(
timestamp, iyaw.tolist()))
self._logger.debug("@{}: Frenet D: {}".format(
timestamp, d.tolist()))
self._logger.debug("@{}: Frenet S: {}".format(
timestamp, s.tolist()))
self._logger.debug("@{}: Frenet Speeds X: {}".format(
timestamp, speeds_x.tolist()))
self._logger.debug("@{}: Frenet Speeds Y: {}".format(
timestamp, speeds_y.tolist()))
self._logger.debug("@{}: Frenet Costs: {}".format(
timestamp, costs))
# update current pose
self.s0 = misc['s']
# log debug
initial_conditions['pos'] = initial_conditions['pos'].tolist()
initial_conditions['vel'] = initial_conditions['vel'].tolist()
initial_conditions['wp'] = initial_conditions['wp'].tolist()
initial_conditions['obs'] = initial_conditions['obs'].tolist()
self._logger.debug("@{}: Frenet Initial Conditions: {}".format(
timestamp, misc))
self._logger.debug("@{}: Euclidean Initial Conditions: {}".format(
timestamp, initial_conditions))
self._logger.debug("@{}: Hyperparameters: {}".format(
timestamp, self._hyperparameters))
# construct and send waypoint message
waypoints_message = self._construct_waypoints(timestamp, pose_msg,
path_x, path_y, speeds,
success)
waypoints_stream.send(waypoints_message)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
pose_msg = self._pose_msgs.popleft()
vehicle_transform = pose_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self.build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
self._prev_waypoints = self._waypoints
else:
# haven't received waypoints from global trajectory stream
self._logger.debug(
"@{}: Sending target speed 0, haven't"
"received global trajectory".format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# if no obstacles, don't use RRT*
if (self._flags.track == -1 or self._flags.track == 3) and len(obstacle_list) == 0:
start = np.array([
self._vehicle_transform.location.x,
self._vehicle_transform.location.y
])
# find the closest point to current location
mindex = self._get_closest_index(start)
path_x = []
path_y = []
for wp in itertools.islice(self._waypoints, mindex,
mindex + self._flags.num_waypoints_ahead):
path_x.append(wp.location.x)
path_y.append(wp.location.y)
path_x = np.array(path_x)
path_y = np.array(path_y)
success = 1
else:
# run rrt star
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
path_x, path_y, success = self._apply_rrt_star(obstacle_list,
self._hyperparameters,
timestamp)
speeds = [0]
if success:
speeds = [self._flags.target_speed] * len(path_x)
self._logger.debug("@{}: RRT* Path X: {}".format(
timestamp, path_x.tolist()))
self._logger.debug("@{}: RRT* Path Y: {}".format(
timestamp, path_y.tolist()))
self._logger.debug("@{}: RRT* Speeds: {}".format(
timestamp, [self._flags.target_speed] * len(path_x)))
# log debug
self._logger.debug("@{}: Hyperparameters: {}".format(
timestamp, self._hyperparameters))
# construct and send waypoint message
waypoint_message = self._construct_waypoints(timestamp, pose_msg,
path_x, path_y,
speeds, success)
waypoints_stream.send(waypoint_message)
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()