def get_control_message(self, waypoints, vehicle_transform, current_speed,
speed_factor, timestamp):
ego_location = vehicle_transform.location.as_carla_location()
ego_yaw = np.deg2rad(zero_to_2_pi(vehicle_transform.rotation.yaw))
# step the controller
self.setup_mpc(waypoints)
self.mpc.vehicle.x = ego_location.x
self.mpc.vehicle.y = ego_location.y
self.mpc.vehicle.yaw = ego_yaw
try:
self.mpc.step()
except Exception as e:
self._logger.info("Failed to solve MPC.")
self._logger.info(e)
return ControlMessage(0, 0, 1, False, False, timestamp)
# compute pid controls
target_speed = self.mpc.solution.vel_list[0] * speed_factor
target_yaw = self.mpc.solution.yaw_list[0]
target_steer_rad = self.mpc.horizon_steer[0] # in rad
steer = self.__rad2steer(target_steer_rad) # [-1.0, 1.0]
throttle, brake = self.__get_throttle_brake_without_factor(
current_speed, target_speed)
# send controls
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self._flags.steer_gain * wp_angle
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self._flags.target_speed * speed_factor / 2
else:
target_speed_adjusted = self._flags.target_speed * speed_factor
self._pid.target = target_speed_adjusted
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, target_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self.__get_steer(wp_angle)
throttle, brake = self.__get_throttle_brake(current_speed,
target_speed,
wp_angle_speed,
speed_factor)
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def on_watermark(self, timestamp, control_stream):
"""Computes and sends the control command on the control 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))
pose_msg = self._pose_msgs.popleft()
vehicle_transform = pose_msg.data.transform
# Vehicle sped in m/s
current_speed = pose_msg.data.forward_speed
waypoint_msg = self._waypoint_msgs.popleft()
if current_speed < 0:
self._logger.warning(
'Current speed is negative: {}'.format(current_speed))
current_speed = 0
waypoints = waypoint_msg.waypoints
self._logger.debug("@{} Received waypoints of length: {}".format(
timestamp, len(waypoints)))
if len(waypoints) > 0:
pid_steer_wp = self._flags.pid_steer_wp
pid_speed_wp = self._flags.pid_speed_wp
if self._flags.carla_mode == "pseudo-asynchronous":
# The control runs at a higher frequency, we need to choose
# based on the current speed.
pid_speed_wp = int(current_speed / 3)
pid_steer_wp = 2 * pid_speed_wp
# The operator picks the wp_num_steer-th waypoint to compute the
# angle it has to steer by when taking a turn.
# Use 10th waypoint for steering.
_, wp_angle_steer = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
vehicle_transform, waypoints, pid_steer_wp)
# Use 5th waypoint for speed.
_, wp_angle_speed = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
vehicle_transform, waypoints, pid_speed_wp)
target_speed = waypoint_msg.target_speeds[min(
len(waypoint_msg.target_speeds) - 1, self._flags.pid_speed_wp)]
throttle, brake = pylot.control.utils.compute_throttle_and_brake(
self._pid, current_speed, target_speed, self._flags)
steer = pylot.control.utils.radians_to_steer(
wp_angle_steer, self._flags.steer_gain)
else:
self._logger.warning('Braking! No more waypoints to follow.')
throttle, brake = 0.0, 0.5
steer = 0.0
self._logger.debug(
'@{}: speed {}, location {}, steer {}, throttle {}, brake {}'.
format(timestamp, current_speed, vehicle_transform, steer,
throttle, brake))
control_stream.send(
ControlMessage(steer, throttle, brake, False, False, timestamp))
def on_can_bus_update(self, msg):
self._latest_speed = msg.data.forward_speed
self._vehicle_transform = msg.data.transform
throttle = 0.0
brake = 0
steer = 0
if self._last_waypoint_msg:
throttle, brake = self._get_throttle_brake(
self._last_waypoint_msg.target_speed)
steer = self._get_steering(self._last_waypoint_msg.waypoints[0])
control_msg = ControlMessage(steer, throttle, brake, False, False,
msg.timestamp)
self.get_output_stream('control_stream').send(control_msg)
def on_watermark(self, timestamp, control_stream):
"""Invoked when the input stream has received a watermark.
The method sends a control message.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
# The control message is ignored by the bridge operator because
# data gathering is conducted using auto pilot. Send default control
# message.
control_msg = ControlMessage(0, 0, 0, False, False, timestamp)
control_stream.send(control_msg)
def get_control_message(self, waypoints, target_speeds, vehicle_transform,
current_speed, timestamp):
self.setup_mpc(waypoints, target_speeds)
self._mpc.vehicle.x = vehicle_transform.location.x
self._mpc.vehicle.y = vehicle_transform.location.y
self._mpc.vehicle.yaw = np.deg2rad(
zero_to_2_pi(vehicle_transform.rotation.yaw))
try:
self._mpc.step()
except Exception as e:
self._logger.error('Failed to solve MPC. Emergency braking.')
self._logger.error(e)
return ControlMessage(0, 0, 1, False, False, timestamp)
# Compute pid controls.
target_speed = self._mpc.solution.vel_list[-1]
target_steer_rad = self._mpc.horizon_steer[0] # in rad
steer = pylot.control.utils.radians_to_steer(target_steer_rad,
self._flags.steer_gain)
throttle, brake = pylot.control.utils.compute_throttle_and_brake(
self._pid, current_speed, target_speed, self._flags, self._logger)
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def on_watermark(self, timestamp: Timestamp, control_stream: WriteStream):
"""Invoked when the input stream has received a watermark.
The method sends a control message.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
# The control message is ignored by the bridge operator because
# data gathering is conducted using auto pilot.
# Send the control that the vehicle is currently applying.
vehicle_control = self._vehicle.get_control()
control_msg = ControlMessage(vehicle_control.steer,
vehicle_control.throttle,
vehicle_control.brake,
vehicle_control.hand_brake,
vehicle_control.reverse, timestamp)
control_stream.send(control_msg)
control_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp: Timestamp, control_stream: WriteStream):
"""Computes and sends the control command on the control 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))
# pose_msg = self._pose_msgs.popleft()
# ego_transform = pose_msg.data.transform
# # Vehicle speed in m/s.
# current_speed = pose_msg.data.forward_speed
# waypoints = self._waypoint_msgs.popleft().waypoints
# try:
# angle_steer = waypoints.get_angle(
# ego_transform, self._flags.min_pid_steer_waypoint_distance)
# target_speed = waypoints.get_target_speed(
# ego_transform, self._flags.min_pid_speed_waypoint_distance)
# throttle, brake = pylot.control.utils.compute_throttle_and_brake(
# self._pid, current_speed, target_speed, self._flags,
# self._logger)
# steer = pylot.control.utils.radians_to_steer(
# angle_steer, self._flags.steer_gain)
# except ValueError:
# self._logger.warning('Braking! No more waypoints to follow.')
# throttle, brake = 0.0, 0.5
# steer = 0.0
# self._logger.debug(
# '@{}: speed {}, location {}, steer {}, throttle {}, brake {}'.
# format(timestamp, current_speed, ego_transform, steer, throttle,
# brake))
steer = 0.0
throttle = 1.0
brake = 0.0
control_stream.send(
ControlMessage(steer, throttle, brake, False, False, timestamp))
control_stream.send(erdos.WatermarkMessage(timestamp))
def on_watermark(self, timestamp, control_stream):
# The control message is ignored by the bridge operator because
# data gathering is conducted using auto pilot. Send default control
# message.
control_msg = ControlMessage(0, 0, 0, False, False, timestamp)
control_stream.send(control_msg)
def on_watermark(self, msg):
control_msg = ControlMessage(
0, 0, 0, False, False, msg.timestamp)
self.get_output_stream('control_stream').send(control_msg)
def run(self):
# Initialize all the publishers.
self.enable_pub = rospy.Publisher(ENABLE_TOPIC, Empty, queue_size=10)
self.disable_pub = rospy.Publisher(DISABLE_TOPIC, Empty, queue_size=10)
self.throttle_pub = rospy.Publisher(THROTTLE_TOPIC,
ThrottleCmd,
queue_size=10)
self.brake_pub = rospy.Publisher(BRAKE_TOPIC, BrakeCmd, queue_size=10)
self.steering_pub = rospy.Publisher(STEERING_TOPIC,
SteeringCmd,
queue_size=10)
rospy.init_node(self.config.name, anonymous=True, disable_signals=True)
# Enable the ADAS.
#self.enable_pub.publish(Empty())
# Pull from the control stream and publish messages continuously.
r = rospy.Rate(ROS_FREQUENCY)
last_control_message = ControlMessage(
steer=0,
throttle=0,
brake=0,
hand_brake=False,
reverse=False,
timestamp=erdos.Timestamp(coordinates=[0]))
while not rospy.is_shutdown():
control_message = self._control_stream.try_read()
if control_message is None or isinstance(control_message,
erdos.WatermarkMessage):
control_message = last_control_message
else:
last_control_message = control_message
# Send all the commands from a single ControlMessage one after
# the other.
steer_angle = control_message.steer * STEERING_ANGLE_MAX
self._logger.debug("The steering angle is {}".format(steer_angle))
steer_message = SteeringCmd(enable=True,
clear=True,
ignore=False,
quiet=False,
count=0,
steering_wheel_angle_cmd=steer_angle,
steering_wheel_angle_velocity=0.0)
self.steering_pub.publish(steer_message)
throttle_message = ThrottleCmd(
enable=True,
ignore=False,
count=0,
pedal_cmd_type=ThrottleCmd.CMD_PERCENT,
pedal_cmd=control_message.throttle)
self.throttle_pub.publish(throttle_message)
brake_message = BrakeCmd(enable=True,
ignore=False,
count=0,
pedal_cmd_type=BrakeCmd.CMD_PERCENT,
pedal_cmd=control_message.brake)
self.brake_pub.publish(brake_message)
# Run at frequency
r.sleep()
def compute_command(self, can_bus_msg, waypoint_msg, tl_msg, obstacles_msg,
pc_msg, depth_msg, timestamp):
start_time = time.time()
vehicle_transform = can_bus_msg.data.transform
vehicle_speed = can_bus_msg.data.forward_speed
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
target_speed = waypoint_msg.target_speed
# Transform point cloud to camera coordinates.
point_cloud = None
if pc_msg:
point_cloud = pylot.simulation.utils.lidar_point_cloud_to_camera_coordinates(
pc_msg.point_cloud)
depth_frame = None
if depth_msg:
depth_frame = depth_msg.frame
traffic_lights = self.__transform_tl_output(tl_msg, vehicle_transform,
point_cloud, depth_frame)
game_time = timestamp.coordinates[0]
if len(traffic_lights) > 0:
self._last_traffic_light_game_time = game_time
(pedestrians,
vehicles) = self.__transform_detector_output(obstacles_msg,
vehicle_transform,
point_cloud, depth_frame)
# if self._map.is_on_opposite_lane(vehicle_transform):
# # Ignore obstacles
# self._logger.info('Ego-vehicle {} on opposite lange'.format(
# vehicle_transform))
# pedestrians = []
# vehicles = []
# traffic_lights = []
self._logger.info('{} Current speed {} and location {}'.format(
timestamp, vehicle_speed, vehicle_transform))
self._logger.info('{} Pedestrians {}'.format(timestamp, pedestrians))
self._logger.info('{} Vehicles {}'.format(timestamp, vehicles))
speed_factor, _ = self.__stop_for_agents(vehicle_transform, wp_angle,
wp_vector, vehicles,
pedestrians, traffic_lights,
timestamp)
new_target_speed = self.reduce_speed_when_approaching_intersection(
vehicle_transform, vehicle_speed, target_speed, game_time)
if new_target_speed != target_speed:
self._logger.info(
'Proximity to intersection, reducing speed from {} to {}'.
format(target_speed, new_target_speed))
target_speed = new_target_speed
self._logger.info('{} Current speed factor {}'.format(
timestamp, speed_factor))
control_msg = self.get_control_message(wp_angle, wp_angle_speed,
speed_factor, vehicle_speed,
target_speed, timestamp)
if control_msg.throttle > 0.001:
self._last_moving_time = game_time
self._num_control_override = 0
if self._num_control_override > 0:
self._num_control_override -= 1
control_msg.throttle = 0.75
# Might be stuck because of a faulty detector.
# Override control message if we haven't been moving for a while.
if game_time - self._last_moving_time > 30000:
self._num_control_override = 6
control_msg = ControlMessage(0, 0.75, 0, False, False, timestamp)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, timestamp,
runtime))
self.get_output_stream('control_stream').send(control_msg)
def on_watermark(self, timestamp, control_stream):
""" The callback function invoked upon receipt of a WatermarkMessage.
The function uses the latest location of the vehicle and drives to the
next waypoint, while doing either a stop or a swerve upon the
detection of a person.
Args:
timestamp: Timestamp for which the WatermarkMessage was received.
control_stream: Output stream on which the callback can write to.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
can_bus_msg = self._can_bus_msgs.popleft()
ground_obstacles_msg = self._ground_obstacles_msgs.popleft()
obstacle_msg = self._obstacle_msgs.popleft()
self._logger.debug(
"The vehicle is travelling at a speed of {} m/s.".format(
can_bus_msg.data.forward_speed))
ego_transform = can_bus_msg.data.transform
ego_location = ego_transform.location
ego_wp = self._map.get_waypoint(ego_location.as_carla_location())
people_obstacles = [
obstacle for obstacle in ground_obstacles_msg.obstacles
if obstacle.label == 'person'
]
# Heuristic to tell us how far away do we detect the person.
for person in people_obstacles:
person_wp = self._map.get_waypoint(
person.transform.location.as_carla_location(),
project_to_road=False)
if person_wp and person_wp.road_id == ego_wp.road_id:
for obstacle in obstacle_msg.obstacles:
if obstacle.label == 'person':
self._csv_logger.info(
"{},{},detected a person {}m away".format(
self.config.name, self.SPEED,
person.distance(ego_transform)))
self._csv_logger.info(
"{},{},vehicle speed {} m/s.".format(
self.config.name, self.SPEED,
can_bus_msg.data.forward_speed))
# Figure out the location of the ego vehicle and compute the next
# waypoint.
if self._goal_reached or ego_location.distance(
self._goal) <= self.GOAL_DISTANCE:
self._logger.info(
"The distance was {} and we reached the goal.".format(
ego_location.distance(self._goal)))
control_stream.send(
ControlMessage(0.0, 0.0, 1.0, True, False, timestamp))
self._goal_reached = True
else:
person_detected = False
for person in people_obstacles:
person_wp = self._map.get_waypoint(
person.transform.location.as_carla_location(),
project_to_road=False)
if person_wp and ego_location.distance(
person.transform.location) <= self.DETECTION_DISTANCE:
person_detected = True
break
if person_detected and self._flags.avoidance_behavior == 'stop':
control_stream.send(
ControlMessage(0.0, 0.0, 1.0, True, False, timestamp))
return
# Get the waypoint that is SAMPLING_DISTANCE away.
sample_distance = self.SAMPLING_DISTANCE if \
ego_transform.forward_vector.x > 0 else \
-1 * self.SAMPLING_DISTANCE
steer_loc = ego_location + pylot.utils.Location(
x=sample_distance, y=0, z=0)
wp_steer = self._map.get_waypoint(steer_loc.as_carla_location())
in_swerve = False
fwd_vector = wp_steer.transform.get_forward_vector()
waypoint_fwd = [fwd_vector.x, fwd_vector.y, fwd_vector.z]
if person_detected:
# If a pedestrian was detected, make sure we're driving on the
# wrong direction.
if np.dot(ego_transform.forward_vector.as_numpy_array(),
waypoint_fwd) > 0:
# We're not driving in the wrong direction, get left
# lane waypoint.
if wp_steer.get_left_lane():
wp_steer = wp_steer.get_left_lane()
in_swerve = True
else:
# We're driving in the right direction, continue driving.
pass
else:
# The person was not detected, come back from the swerve.
if np.dot(ego_transform.forward_vector.as_numpy_array(),
waypoint_fwd) < 0:
# We're driving in the wrong direction, get the left lane
# waypoint.
if wp_steer.get_left_lane():
wp_steer = wp_steer.get_left_lane()
in_swerve = True
else:
# We're driving in the right direction, continue driving.
pass
self._world.debug.draw_point(wp_steer.transform.location,
size=0.2,
life_time=30000.0)
wp_steer_vector, _, wp_steer_angle = \
ego_transform.get_vector_magnitude_angle(
pylot.utils.Location.from_carla_location(
wp_steer.transform.location))
current_speed = max(0, can_bus_msg.data.forward_speed)
steer = pylot.control.utils.radians_to_steer(
wp_steer_angle, self._flags.steer_gain)
# target_speed = self.SPEED if not in_swerve else self.SPEED / 5.0
target_speed = self.SPEED
throttle, brake = pylot.control.utils.compute_throttle_and_brake(
self._pid, current_speed, target_speed, self._flags)
control_stream.send(
ControlMessage(steer, throttle, brake, False, False,
timestamp))
def on_watermark(self, timestamp, control_stream):
"""Computes and sends the control command on the control 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))
pose_msg = self._pose_msgs.popleft()
vehicle_transform = pose_msg.data.transform
# Vehicle sped in m/s
current_speed = pose_msg.data.forward_speed
waypoint_msg = self._waypoint_msgs.popleft()
if current_speed < 0:
self._logger.warning(
'Current speed is negative: {}'.format(current_speed))
current_speed = 0
waypoints = waypoint_msg.waypoints
self._logger.debug("@{} Received waypoints of length: {}".format(
timestamp, len(waypoints)))
if len(waypoints) > 0:
pid_steer_wp, pid_speed_wp = None, None
for index, _wp in enumerate(waypoints):
# Break if we have found both the desired waypoints.
if pid_steer_wp and pid_speed_wp:
break
ego_distance = _wp.location.distance(
vehicle_transform.location)
if pid_steer_wp is None and (ego_distance >
self._flags.pid_steer_wp):
pid_steer_wp = index
if pid_speed_wp is None and (ego_distance >
self._flags.pid_speed_wp):
pid_speed_wp = index
if pid_steer_wp is None:
pid_steer_wp = -1
if pid_speed_wp is None:
pid_speed_wp = -1
_, wp_angle_steer = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
vehicle_transform, waypoints, pid_steer_wp)
# Use 5th waypoint for speed.
_, wp_angle_speed = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
vehicle_transform, waypoints, pid_speed_wp)
target_speed = waypoint_msg.target_speeds[min(
len(waypoint_msg.target_speeds) - 1, self._flags.pid_speed_wp)]
throttle, brake = pylot.control.utils.compute_throttle_and_brake(
self._pid, current_speed, target_speed, self._flags)
steer = pylot.control.utils.radians_to_steer(
wp_angle_steer, self._flags.steer_gain)
else:
self._logger.warning('Braking! No more waypoints to follow.')
throttle, brake = 0.0, 0.5
steer = 0.0
self._logger.debug(
'@{}: speed {}, location {}, steer {}, throttle {}, brake {}'.
format(timestamp, current_speed, vehicle_transform, steer,
throttle, brake))
control_stream.send(
ControlMessage(steer, throttle, brake, False, False, timestamp))
