def IDM_speed_in_lane(self, lane):
ego_vehicle_location = np.array([self.dynamic_map.ego_state.pose.pose.position.x,
self.dynamic_map.ego_state.pose.pose.position.y])
v = get_speed(self.dynamic_map.ego_state)
v0 = lane.map_lane.speed_limit/3.6
if v < 5:
a = self.a + (5 - v)/5*2
else:
a = self.a
b = self.b
g0 = self.g0
T = self.T
delta = self.delta
if len(lane.front_vehicles) > 0:
f_v_location = np.array([
lane.front_vehicles[0].state.pose.pose.position.x,
lane.front_vehicles[0].state.pose.pose.position.y
])
v_f = get_speed(lane.front_vehicles[0].state) # TODO: get mmap vx and vy, the translator part in nearest locator
dv = v - v_f
g = np.linalg.norm(f_v_location - ego_vehicle_location)
g1 = g0 + T*v + v*dv/(2*np.sqrt(a*b))
else:
dv = 0
g = 50
g1 = 0
acc = a*(1 - pow(v/v0, delta) - (g1/g)*((g1/g)))
return max(0, v + acc*self.decision_dt)
def run_step(self, msg):
"""
Execute one step of control invoking both lateral and longitudinal PID controllers to track a trajectory
at a given target_speed.
return: control
"""
self.ego_state = msg.state
rospy.logdebug("received target speed:%f, current_speed: %f", self.desired_speed, get_speed(self.ego_state))
if not self.ready_for_control():
control_msg = ControlCommand()
control_msg.accel = -1
control_msg.steer = 0
return control_msg
target_speed = self.desired_speed
trajectory = self.desired_trajectory
current_speed = get_speed(self.ego_state)
ego_pose = self.ego_state.pose.pose
accel = self._lon_controller.run_step(target_speed, current_speed)
steer = self._lat_controller.run_step(ego_pose, trajectory, current_speed)
rospy.logdebug("accel = %f, steer = %f", accel, steer)
control_msg = ControlCommand()
control_msg.accel = accel
control_msg.steer = steer
return control_msg
def get_decision_from_discrete_action(self,
action,
acc=2,
decision_dt=0.75,
hard_brake=4):
# TODO(Zhong): check if action is reasonable
# Rule-based action
if action == 0:
return self._rule_based_lateral_model_instance.lateral_decision(
self._dynamic_map)
current_speed = get_speed(self._dynamic_map.ego_state)
ego_y = self._dynamic_map.mmap.ego_lane_index
# Hard-brake action
if action == 1:
return ego_y, current_speed - hard_brake * decision_dt
# ego lane action
if action == 2:
return ego_y, current_speed + acc * decision_dt
if action == 3:
return ego_y, current_speed
if action == 4:
return ego_y, current_speed - acc * decision_dt
# left lane action
left_lane_index = ego_y + 1
if left_lane_index >= len(self._dynamic_map.mmap.lanes):
if action == 5 or action == 6 or action == 7:
return self._rule_based_lateral_model_instance.lateral_decision(
self._dynamic_map)
if action == 5:
return left_lane_index, current_speed + acc * decision_dt
if action == 6:
return left_lane_index, current_speed
if action == 7:
return left_lane_index, current_speed - acc * decision_dt
#right_lane_action
right_lane_index = ego_y - 1
if right_lane_index < 0:
if action == 8 or action == 9 or action == 10:
return self._rule_based_lateral_model_instance.lateral_decision(
self._dynamic_map)
if action == 8:
return right_lane_index, current_speed + acc * decision_dt
if action == 9:
return right_lane_index, current_speed
if action == 10:
return right_lane_index, current_speed - acc * decision_dt
def traffic_light_speed(self, lane):
ego_vehicle_speed = get_speed(self.dynamic_map.ego_state)
if lane.map_lane.stop_state == Lane.STOP_STATE_THRU:
return float("inf")
else:
d = lane.stop_distance
if d < 10 + ego_vehicle_speed*ego_vehicle_speed/2/2:
return 0
return float("inf")
def pred_trajectory(self, obstacle, pred_t=4, resolution=0.1):
# Assuming constant speed
loc = np.array([
obstacle.state.pose.pose.position.x,
obstacle.state.pose.pose.position.y
])
speed = get_speed(obstacle.state)
yaw = get_yaw(obstacle.state)
speed_direction = np.array([np.cos(yaw), np.sin(yaw)])
t_space = np.linspace(0, pred_t, pred_t / resolution)
pred_x = loc[0] + t_space * speed * speed_direction[0]
pred_y = loc[1] + t_space * speed * speed_direction[1]
pred_trajectory = np.array([pred_x, pred_y]).T
return pred_trajectory
def intersection_between_trajectories(self,
decision_trajectory,
pred_trajectory,
vehicle,
collision_thres=4,
stop_thres=3,
unavoidable_distance=4):
'''
If two trajectory have interection, return the distance
TODO: Implement this method into a standalone library
'''
nearest_idx = len(decision_trajectory) - 1
collision_time = float("inf")
surrounding_vehicle_speed = get_speed(vehicle.state)
# the object stops
# if np.linalg.norm(pred_trajectory[0] - pred_trajectory[-1]) < stop_thres:
if surrounding_vehicle_speed < stop_thres:
return nearest_idx, collision_time
ego_pose = self.dynamic_map.ego_state.pose.pose
ego_loc = np.array([ego_pose.position.x, ego_pose.position.y])
for wp in pred_trajectory:
dist = np.linalg.norm(decision_trajectory - wp, axis=1)
min_d = np.min(dist)
distance_to_ego_vehicle = np.linalg.norm(wp - ego_loc)
if distance_to_ego_vehicle < unavoidable_distance:
break
if min_d < collision_thres:
nearest_idx = np.argmin(dist)
surrounding_vehicle_distance_before_collision = dist_from_point_to_polyline(
wp[0], wp[1], pred_trajectory)[1]
collision_time = surrounding_vehicle_distance_before_collision / surrounding_vehicle_speed
# rospy.logdebug("considering vehicle:%d, dis_to_collision:%d, col_time:%.2f, speed:%.2f col_index:%d(%d)",vehicle.uid,
# surrounding_vehicle_distance_before_collision,
# collision_time,surrounding_vehicle_speed,
# nearest_idx,len(decision_trajectory))
break
return nearest_idx, collision_time
def lane_change_safe(self, ego_lane_index, target_index):
if target_index < 0 or target_index > len(
self.dynamic_map.mmap.lanes) - 1:
return False
front_safe = False
rear_safe = False
front_vehicle = None
rear_vehicle = None
ego_vehicle_location = np.array([
self.dynamic_map.ego_state.pose.pose.position.x,
self.dynamic_map.ego_state.pose.pose.position.y
])
for lane in self.dynamic_map.mmap.lanes:
if lane.map_lane.index == target_index:
if len(lane.front_vehicles) > 0:
front_vehicle = lane.front_vehicles[0]
if len(lane.rear_vehicles) > 0:
rear_vehicle = lane.rear_vehicles[0]
break
ego_v = get_speed(self.dynamic_map.ego_state)
if front_vehicle is None:
d_front = -1
front_safe = True
behavior_front = RoadObstacle.BEHAVIOR_UNKNOWN
else:
front_vehicle_location = np.array([
front_vehicle.state.pose.pose.position.x,
front_vehicle.state.pose.pose.position.y
])
behavior_front = front_vehicle.behavior
# TODO: Change to real distance in lane
d_front = np.linalg.norm(front_vehicle_location -
ego_vehicle_location)
front_v = get_speed(front_vehicle.state)
if d_front > max(10 + 3 * (ego_v - front_v), 10):
front_safe = True
if rear_vehicle is None:
rear_safe = True
d_rear = -1
behavior_rear = RoadObstacle.BEHAVIOR_UNKNOWN
else:
rear_vehicle_location = np.array([
rear_vehicle.state.pose.pose.position.x,
rear_vehicle.state.pose.pose.position.y
])
behavior_rear = rear_vehicle.behavior
d_rear = np.linalg.norm(rear_vehicle_location -
ego_vehicle_location)
rear_v = get_speed(rear_vehicle.state)
if d_rear > max(10 + 3 * (rear_v - ego_v), 10):
rear_safe = True
rospy.logdebug(
"ego_lane = %d, target_lane = %d, front_d = %f(%d), rear_d = %f(%d)",
ego_lane_index, target_index, d_front, behavior_front, d_rear,
behavior_rear)
if front_safe and rear_safe:
return True
return False