def convertToLocal(cur_vehicle: VehicleState, adj_vehicle: VehicleState): """ Converts the adj_vehicle position into cur_vehicle position Expects VehicleState objects and Euclidean Coordinate Frame Args :param cur_vehicle: (VehicleState) the ego vehicle state :param adj_vehicle: (VehicleState) the target vehicle state Returns the adj_vehicle pose in terms of the cur_vehicle """ # create a place holder vehicle object to represent the resultant relative pose result_state = VehicleState() # extract the pose and velocities of the adj_vehicle x = adj_vehicle.vehicle_location.x y = adj_vehicle.vehicle_location.y theta = adj_vehicle.vehicle_location.theta speed = adj_vehicle.vehicle_speed # heading is in terms of the x-axis so vx is cos component # vx = speed * np.cos(theta) # vy = speed * np.sin(theta) # get current_vehicle_speeds # cvx = cur_vehicle.vehicle_speed * np.cos(cur_vehicle.vehicle_location.theta) # cvy = cur_vehicle.vehicle_speed * np.sin(cur_vehicle.vehicle_location.theta) # make homogeneous transform matrix to transform global coordinate into the frame # of the ego vehicle state. Calculate as a product of two transformations. Multiply right. H_Rot = np.eye(3) H_Rot[-1, -1] = 1 H_Rot[0, -1] = 0 H_Rot[1, -1] = 0 H_Rot[0, 0] = np.cos(cur_vehicle.vehicle_location.theta) H_Rot[0, 1] = -np.sin(cur_vehicle.vehicle_location.theta) H_Rot[1, 0] = np.sin(cur_vehicle.vehicle_location.theta) H_Rot[1, 1] = np.cos(cur_vehicle.vehicle_location.theta) H_trans = np.eye(3) H_trans[0, -1] = -cur_vehicle.vehicle_location.x H_trans[1, -1] = -cur_vehicle.vehicle_location.y H = np.matmul(H_Rot, H_trans) # calculate and set relative position res = np.matmul(H, np.array([x, y, 1]).reshape(3, 1)) result_state.vehicle_location.x = res[0, 0] result_state.vehicle_location.y = res[1, 0] # calculate and set relative orientation result_state.vehicle_location.theta = theta - cur_vehicle.vehicle_location.theta # calculate and set relative speed # res_vel = np.array([vx - cvx, vy - cvy]) result_state.vehicle_speed = speed # np.linalg.norm(res_vel) # time.sleep(5) return result_state
def convert2ROS(self): def speed_conversion(sim_speed): return sim_speed * 3.6 vehicle_state = VehicleState() vehicle_state.vehicle_location.x = self.x vehicle_state.vehicle_location.y = self.y vehicle_state.vehicle_location.theta = self.theta vehicle_state.length = self.length vehicle_state.width = self.width vehicle_state.vehicle_speed = speed_conversion(self.speed) return vehicle_state
def terminate(self, env_desc): if self.event == Scenario.LANE_CHANGE: return env_desc.reward.collision or \ env_desc.reward.path_planner_terminate or \ env_desc.reward.time_elapsed > self.eps_time elif self.event == Scenario.PEDESTRIAN: # return true if vehicle has moved past the vehicle ped_vehicle = VehicleState() relative_pose = ped_vehicle if env_desc.nearest_pedestrian.exist: ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed relative_pose = convert_to_local(env_desc.cur_vehicle_state, ped_vehicle) if relative_pose.vehicle_location.x < -1: return True # usual conditions return env_desc.reward.collision or \ env_desc.reward.path_planner_terminate or \ env_desc.reward.time_elapsed > self.eps_time
def getVehicleState(self, actor): if actor == None: return None vehicle = VehicleState() vehicle.vehicle_location.x = actor.get_transform().location.x vehicle.vehicle_location.y = actor.get_transform().location.y vehicle.vehicle_location.theta = (actor.get_transform().rotation.yaw * np.pi / 180 ) # CHECK : Changed this to radians. vehicle.vehicle_speed = ( np.sqrt(actor.get_velocity().x**2 + actor.get_velocity().y**2 + actor.get_velocity().z**2) * 3.6) vehicle_bounding_box = actor.bounding_box.extent vehicle.length = vehicle_bounding_box.x * 2 vehicle.width = vehicle_bounding_box.y * 2 return vehicle
def make_state_vector(self, data): ''' create a state vector from the message recieved ''' env_state = [] def append_vehicle_state(env_state, vehicle_state): env_state.append(vehicle_state.vehicle_location.x) env_state.append(vehicle_state.vehicle_location.y) env_state.append(vehicle_state.vehicle_location.theta) env_state.append(vehicle_state.vehicle_speed) return # items needed # current vehicle velocity append_vehicle_state(env_state, data.cur_vehicle_state) #env_state.append(data.cur_vehicle_state.vehicle_speed) # rear vehicle state # position # velocity #append_vehicle_state(env_state, data.back_vehicle_state) #append_vehicle_state(env_state, data.front_vehicle_state) # adjacent vehicle state # position # velocity i = 0 for _, veh_state in enumerate(data.adjacent_lane_vehicles): if i < 5: append_vehicle_state(env_state, veh_state) else: break i += 1 dummy = VehicleState() dummy.vehicle_location.x = data.cur_vehicle_state.vehicle_location.x + 100 dummy.vehicle_location.y = data.cur_vehicle_state.vehicle_location.y + 100 dummy.vehicle_location.theta = 0 dummy.vehicle_speed = 0 while i < 5: append_vehicle_state(env_state, dummy) i += 1 return env_state
def speed_reward(self, desc, action): reward = 0 self.min_dist = desc.nearest_pedestrian.radius ped_vehicle = VehicleState() relative_pose = ped_vehicle if desc.nearest_pedestrian.exist: ped_vehicle.vehicle_location = desc.nearest_pedestrian.pedestrian_location ped_vehicle.vehicle_speed = desc.nearest_pedestrian.pedestrian_speed relative_pose = convertToLocal(desc.cur_vehicle_state, ped_vehicle) # check if pedestrian collided if desc.reward.collision: return -self.max_reward # check if pedestrian avoided elif desc.nearest_pedestrian.exist and relative_pose.vehicle_location.x < -10: reward = self.max_reward # add costs of overspeeding reward -= self.vel_cost() reward -= self.position_cost() self.reset() return reward
def convert_to_local(cur_vehicle, adj_vehicle): result_state = VehicleState() x = adj_vehicle.vehicle_location.x y = adj_vehicle.vehicle_location.y theta = adj_vehicle.vehicle_location.theta speed = adj_vehicle.vehicle_speed vx = speed * np.cos(theta) vy = speed * np.sin(theta) # get current_vehicle_speeds cvx = cur_vehicle.vehicle_speed * np.cos( cur_vehicle.vehicle_location.theta) cvy = cur_vehicle.vehicle_speed * np.sin( cur_vehicle.vehicle_location.theta) # make homogeneous transform H_Rot = np.eye(3) H_Rot[-1, -1] = 1 H_Rot[0, -1] = 0 H_Rot[1, -1] = 0 H_Rot[0, 0] = np.cos(cur_vehicle.vehicle_location.theta) H_Rot[0, 1] = -np.sin(cur_vehicle.vehicle_location.theta) H_Rot[1, 0] = np.sin(cur_vehicle.vehicle_location.theta) H_Rot[1, 1] = np.cos(cur_vehicle.vehicle_location.theta) H_trans = np.eye(3) H_trans[0, -1] = -cur_vehicle.vehicle_location.x H_trans[1, -1] = -cur_vehicle.vehicle_location.y H = np.matmul(H_Rot, H_trans) # calculate and set relative position res = np.matmul(H, np.array([x, y, 1]).reshape(3, 1)) result_state.vehicle_location.x = res[0, 0] result_state.vehicle_location.y = res[1, 0] # calculate and set relative orientation result_state.vehicle_location.theta = theta - cur_vehicle.vehicle_location.theta # calculate and set relative speed res_vel = np.array([vx - cvx, vy - cvy]) result_state.vehicle_speed = speed # np.linalg.norm(res_vel) # time.sleep(5) return result_state
def terminate(self, env_desc: EnvironmentState) -> bool: """ A function which returns true if the episode must terminate. This is decided by the following conditions. * Has the vehicle collinded? * Is the episode time elapsed more than the threshold self.eps_time? * Is the maneouver completed? Args: env_desc: (EnvironmentState) ROS Message for the environment Returns: true if episode must terminate """ if self.event == Scenario.LANE_CHANGE: # return true if any of the conditions described in the description is true return env_desc.reward.collision or \ env_desc.reward.path_planner_terminate or \ env_desc.reward.time_elapsed > self.eps_time elif self.event == Scenario.PEDESTRIAN: # return true if vehicle has moved past the pedestrian # treat a pedestrian as a vehicle object ped_vehicle = VehicleState() relative_pose = VehicleState() # if there is a pedestrian closeby then check if we have passed him if env_desc.nearest_pedestrian.exist: # populate the "pedestrian vehicle" parameters ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed relative_pose = convertToLocal(env_desc.cur_vehicle_state,ped_vehicle) if relative_pose.vehicle_location.x < -10: return True # usual conditions return env_desc.reward.collision or \ env_desc.reward.path_planner_terminate or \ env_desc.reward.time_elapsed > self.eps_time
def make_state_vector(self, data): ''' create a state vector from the message recieved ''' i = 0 env_state = [] self.append_vehicle_state(env_state, data.cur_vehicle_state) # self.append_vehicle_state(env_state, data.back_vehicle_state) # self.append_vehicle_state(env_state, data.front_vehicle_state) for _, veh_state in enumerate(data.adjacent_lane_vehicles): if i < 5: self.append_vehicle_state(env_state, veh_state) else: break i += 1 dummy = VehicleState() dummy.vehicle_location.x = 100 dummy.vehicle_location.y = 100 dummy.vehicle_location.theta = 0 dummy.vehicle_speed = 0 while i < 5: self.append_vehicle_state(env_state, dummy) i += 1 return env_state
def makeStateVector(self, data, local=False): ''' create a state vector from the message recieved ''' if self.event == Scenario.LANE_CHANGE: i = 0 env_state = [] if not local: self.append_vehicle_state(env_state, data.cur_vehicle_state) # self.append_vehicle_state(env_state, data.back_vehicle_state) # self.append_vehicle_state(env_state, data.front_vehicle_state) for _, vehicle in enumerate(data.adjacent_lane_vehicles): if i < 5: self.append_vehicle_state(env_state, vehicle) else: break i += 1 else: cur_vehicle_state = VehicleState() cur_vehicle_state.vehicle_location.x = 0 cur_vehicle_state.vehicle_location.y = 0 cur_vehicle_state.vehicle_location.theta = 0 cur_vehicle_state.vehicle_speed = data.cur_vehicle_state.vehicle_speed self.append_vehicle_state(env_state, cur_vehicle_state) for _, vehicle in enumerate(data.adjacent_lane_vehicles): converted_state = convert_to_local(data.cur_vehicle_state, vehicle) if i < 5: self.append_vehicle_state(env_state, converted_state) else: break i += 1 dummy = VehicleState() dummy.vehicle_location.x = 100 dummy.vehicle_location.y = 100 dummy.vehicle_location.theta = 0 dummy.vehicle_speed = 0 while i < 5: self.append_vehicle_state(env_state, dummy) i += 1 return env_state elif self.event == Scenario.PEDESTRIAN: env_state = [] if not local: self.append_vehicle_state(env_state, data.cur_vehicle_state) ped_vehicle = VehicleState() ped_vehicle.vehicle_location = data.nearest_pedestrian.pedestrian_location ped_vehicle.pedestrian_speed = data.nearest_pedestrian.pedestrian_speed self.append_vehicle_state(env_state, ped_vehicle) else: cur_vehicle_state = VehicleState() cur_vehicle_state.vehicle_location.x = 0 cur_vehicle_state.vehicle_location.y = 0 cur_vehicle_state.vehicle_location.theta = 0 cur_vehicle_state.vehicle_speed = data.cur_vehicle_state.vehicle_speed self.append_vehicle_state(env_state, cur_vehicle_state) ped_vehicle = VehicleState() ped_vehicle.vehicle_location = data.nearest_pedestrian.pedestrian_location ped_vehicle.vehicle_speed = data.nearest_pedestrian.pedestrian_speed converted_state = convert_to_local(data.cur_vehicle_state, ped_vehicle) self.append_vehicle_state(env_state, converted_state) return env_state
def embedState(self, env_desc: EnvironmentState, scenario: Scenario, local=False) -> np.ndarray: """ Create a state embedding vector from message recieved Args: :param env_des: (EnvironmentState) a ROS message describing the environment """ # TODO: Convert the embedding to Frenet coordinate system # based on scenario create the embedding accordingly if self.event == Scenario.LANE_CHANGE: # initialize count of vehicles. We will add 5 vehicles # The lane change scenario contains the current vehicle and 5 adjacent vehicles in the state count_of_vehicles = 0 env_state = [] # if local flag is not set then use absolute coordinates if not local: self.append_vehicle_state(env_state, env_desc.cur_vehicle_state) # TODO: Currently not adding vehicles in the front and back. Might need to add these later. # self.append_vehicle_state(env_state, env_desc.back_vehicle_state) # self.append_vehicle_state(env_state, env_desc.front_vehicle_state) # count += 2 # add vehicles in the adjacent lane for _, vehicle in enumerate(env_desc.adjacent_lane_vehicles): if count_of_vehicles < 5: self.append_vehicle_state(env_state, vehicle) else: break count_of_vehicles += 1 else: # use relative coordinates. Current vehicle is origin cur_vehicle_state = VehicleState() cur_vehicle_state.vehicle_location.x = 0 cur_vehicle_state.vehicle_location.y = 0 cur_vehicle_state.vehicle_location.theta = 0 # Still choose absolute current_vehicle speed wrt global frame cur_vehicle_state.vehicle_speed = env_desc.cur_vehicle_state.vehicle_speed self.append_vehicle_state(env_state, cur_vehicle_state) # TODO: Currently not adding vehicles in the front and back. Might need to add these later. # add vehicles in the adjacent lane for _, vehicle in enumerate(env_desc.adjacent_lane_vehicles): converted_state = convertToLocal(env_desc.cur_vehicle_state, vehicle) if count_of_vehicles < 5: self.append_vehicle_state(env_state, converted_state) else: break count_of_vehicles += 1 # add dummy vehicles into the state if not reached 5 dummy = VehicleState() dummy.vehicle_location.x = 100 dummy.vehicle_location.y = 100 dummy.vehicle_location.theta = 0 dummy.vehicle_speed = 0 while count_of_vehicles < 5: self.append_vehicle_state(env_state, dummy) count_of_vehicles += 1 return env_state elif self.event == Scenario.PEDESTRIAN: # initialize an empty list for the state env_state = [] # choose between local and absolute coordinate system # the state embedding contains the vehicle and the pedestrian if not local: self.append_vehicle_state(env_state, env_desc.cur_vehicle_state) ped_vehicle = VehicleState() ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location ped_vehicle.pedestrian_speed = env_desc.nearest_pedestrian.pedestrian_speed self.append_vehicle_state(env_state, ped_vehicle) else: cur_vehicle_state = VehicleState() cur_vehicle_state.vehicle_location.x = 0 cur_vehicle_state.vehicle_location.y = 0 cur_vehicle_state.vehicle_location.theta = 0 cur_vehicle_state.vehicle_speed = env_desc.cur_vehicle_state.vehicle_speed self.append_vehicle_state(env_state, cur_vehicle_state) ped_vehicle = VehicleState() ped_vehicle.vehicle_location = env_desc.nearest_pedestrian.pedestrian_location ped_vehicle.vehicle_speed = env_desc.nearest_pedestrian.pedestrian_speed converted_state = convertToLocal(env_desc.cur_vehicle_state, ped_vehicle) self.append_vehicle_state(env_state, converted_state) return env_state
def talker(): # initialize publishers pub = rospy.Publisher(SIM_TOPIC_NAME, EnvironmentState, queue_size=10) pub_rl = rospy.Publisher(RL_TOPIC_NAME, RLCommand, queue_size=10) rospy.init_node(NODE_NAME, anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): # current Lane lane_cur = Lane() lane_points = [] for i in range(100): lane_point = LanePoint() lane_point.pose.y = 0 lane_point.pose.x = i / 10. lane_point.pose.theta = 0 lane_point.width = 3 lane_points.append(lane_point) lane_cur.lane = lane_points # next Lane lane_next = Lane() lane_points_next = [] for i in range(100): lane_point = LanePoint() lane_point.pose.y = 3 lane_point.pose.x = i / 10. lane_point.pose.theta = 0 lane_point.width = 3 lane_points_next.append(lane_point) lane_next.lane = lane_points_next # current vehicle vehicle = VehicleState() vehicle.vehicle_location.x = 2 vehicle.vehicle_location.y = 0 vehicle.vehicle_location.theta = 0 vehicle.vehicle_speed = 10 vehicle.length = 5 vehicle.width = 2 # sample enviroment state env_state = EnvironmentState() env_state.cur_vehicle_state = copy.copy(vehicle) env_state.front_vehicle_state = copy.copy(vehicle) env_state.back_vehicle_state = copy.copy(vehicle) env_state.adjacent_lane_vehicles = [ copy.copy(vehicle), copy.copy(vehicle) ] env_state.max_num_vehicles = 2 env_state.speed_limit = 40 env_state.current_lane = copy.copy(lane_cur) env_state.next_lane = copy.copy(lane_next) # sample RL command rl_command = RLCommand() rl_command.change_lane = 1 rospy.loginfo("Publishing") pub.publish(env_state) pub_rl.publish(rl_command) rate.sleep()
def initialize(self): # initialize node rospy.init_node(NODE_NAME, anonymous=True) # initialize service self.planner_service = rospy.Service(SIM_SERVICE_NAME, SimService, self.pathRequest) # Start Client. Make sure Carla server is running before starting. client = carla.Client("localhost", 2000) client.set_timeout(2.0) print("Connection to CARLA server established!") # Create a CarlaHandler object. CarlaHandler provides some cutom built APIs for the Carla Server. self.carla_handler = CarlaHandler(client) self.client = client settings = self.carla_handler.world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = self.simulation_sync_timestep self.carla_handler.world.apply_settings(settings) self.tm = CustomScenario(self.client, self.carla_handler) # Reset Environment self.resetEnv() # Initialize PID Controller self.vehicle_controller = GRASPPIDController( self.ego_vehicle, args_lateral={ "K_P": 0.5, "K_D": 0, "K_I": 0, "dt": self.simulation_sync_timestep, }, args_longitudinal={ "K_P": 1, "K_D": 0.0, "K_I": 0.0, "dt": self.simulation_sync_timestep, }, ) state_information = self.carla_handler.get_state_information_new( self.ego_vehicle, self.original_lane) ( current_lane_waypoints, left_lane_waypoints, right_lane_waypoints, front_vehicle, rear_vehicle, actors_in_current_lane, actors_in_left_lane, actors_in_right_lane, ) = state_information self.current_lane = current_lane_waypoints self.left_lane = left_lane_waypoints # self.carla_handler.draw_waypoints(current_lane_waypoints, life_time=100) # self.carla_handler.draw_waypoints(left_lane_waypoints, life_time=100, color=True) pedestrians_on_current_road = self.carla_handler.get_pedestrian_information( self.ego_vehicle) ############################################################################################################## # publish the first frame # Current Lane self.lane_cur = self.getLanePoints(current_lane_waypoints) # Left Lane self.lane_left = self.getLanePoints(left_lane_waypoints) # Right Lane self.lane_right = self.getLanePoints(right_lane_waypoints) vehicle_ego = self.getVehicleState(self.ego_vehicle) # Front vehicle if front_vehicle == None: vehicle_front = VehicleState() # vehicle_ego else: vehicle_front = self.getVehicleState(front_vehicle) # Rear vehicle if rear_vehicle == None: vehicle_rear = VehicleState() # vehicle_ego else: vehicle_rear = self.getVehicleState(rear_vehicle) # Contruct enviroment state ROS message env_state = EnvironmentState() env_state.cur_vehicle_state = vehicle_ego env_state.front_vehicle_state = vehicle_front env_state.back_vehicle_state = vehicle_rear env_state.adjacent_lane_vehicles, _ = self.getClosest( [self.getVehicleState(actor) for actor in actors_in_left_lane], vehicle_ego, self.max_num_vehicles, ) # TODO : Only considering left lane for now. Need to make this more general env_state.current_lane = self.lane_cur env_state.next_lane = self.lane_left env_state.max_num_vehicles = self.max_num_vehicles env_state.speed_limit = 20 ## Pedestrian if self.pedestrian is not None: env_state.nearest_pedestrian = self.getClosestPedestrian( [ self.getPedestrianState(actor) for actor in [self.pedestrian] ], vehicle_ego, 1, )[0] else: env_state.nearest_pedestrian = Pedestrian() env_state.nearest_pedestrian.exist = False reward_info = RewardInfo() reward_info.time_elapsed = self.timestamp reward_info.new_run = self.first_run reward_info.collision = self.collision_marker env_state.reward = reward_info
def updateMessages(self): self.lock.acquire() publish_msg = EnvironmentState() # current lane publish_msg.current_lane = self.lanes[self.cur_lane].convert2ROS() # next lane next_lane = self.cur_lane - 1 if next_lane >= 0: publish_msg.next_lane = self.lanes[next_lane].convert2ROS() # vehicles closest_next_lane_vehicles = [] front_veh = False back_veh = False def sort_veh(veh_tup): return veh_tup[1] veh_dir_vec = np.array(np.cos(self.controlling_vehicle.theta), -np.sin(self.controlling_vehicle.theta)) for veh in self.vehicles: veh_dist = np.linalg.norm( np.array(veh.x - self.controlling_vehicle.x, veh.y - self.controlling_vehicle.y)) if veh.lane == next_lane: # next lane vehicles if len(closest_next_lane_vehicles) < NUM_NEXT_LANE_VEHICLES: closest_next_lane_vehicles.append(tuple((veh, veh_dist))) closest_next_lane_vehicles.sort(key=sort_veh) elif veh_dist < closest_next_lane_vehicles[-1][1]: closest_next_lane_vehicles[-1] = tuple((veh, veh_dist)) closest_next_lane_vehicles.sort(key=sort_veh) if veh.lane == self.cur_lane: x_diff = veh.x - self.controlling_vehicle.x y_diff = veh.y - self.controlling_vehicle.y pos_diff = np.array(x_diff, y_diff) proj = np.dot(veh_dir_vec, pos_diff) if proj > 0: # front vehicle if front_veh: if front_veh[1] > veh_dist: front_veh = tuple((veh, veh_dist)) else: front_veh = tuple((veh, veh_dist)) else: # back vehicle if back_veh: if back_veh[1] > veh_dist: back_veh = tuple((veh, veh_dist)) else: back_veh = tuple((veh, veh_dist)) # next lane vehicle documentation for veh_tup in closest_next_lane_vehicles: publish_msg.adjacent_lane_vehicles.append(veh_tup[0].convert2ROS()) # front vehicle if front_veh: publish_msg.front_vehicle_state = front_veh[0].convert2ROS() else: # ToDo: no front vehicle publish_msg.front_vehicle_state = VehicleState() # back vehicle if back_veh: publish_msg.back_vehicle_state = back_veh[0].convert2ROS() else: # ToDo: no back vehicle publish_msg.back_vehicle_state = VehicleState() # ego vehicle publish_msg.cur_vehicle_state = self.controlling_vehicle.convert2ROS() # max # vehicles publish_msg.max_num_vehicles = NUM_NEXT_LANE_VEHICLES # speed limit publish_msg.speed_limit = 20 # reward info reward_info = RewardInfo() reward_info.time_elapsed = self.timestamp reward_info.new_run = self.first_run reward_info.collision = self.collisionCheck() reward_info.action_progress = self.action_progress reward_info.end_of_action = self.end_of_action publish_msg.reward = reward_info self.env_msg = publish_msg self.lock.release()