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