def apply_physics(self, phys_settings): self.world.tick() """ Default wheel settings: front_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7) front_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7) rear_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0) rear_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0) """ front_left_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['flwf'], damping_rate=0.25, max_steer_angle=phys_settings['flwmsa'], radius=36.7) front_right_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['frwf'], damping_rate=0.25, max_steer_angle=phys_settings['frwmsa'], radius=36.7) rear_left_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['rlwf'], damping_rate=0.25, max_steer_angle=0.0, radius=36.0) rear_right_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['rrwf'], damping_rate=0.25, max_steer_angle=0.0, radius=36.0) wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel] # Change Vehicle Physics Control parameters of the vehicle physics_control = self.player.get_physics_control() #physics_control.max_rpm = phys_settings['max_rpm'] physics_control.mass = phys_settings['mass'] #physics_control.drag_coefficient = phys_settings['drag_coefficient'] physics_control.wheels = wheels physics_control.steering_curve = [carla.Vector2D(0, 1), carla.Vector2D(20, phys_settings['steer1']), carla.Vector2D(60, phys_settings['steer2']), carla.Vector2D(120, phys_settings['steer3'])] physics_control.torque_curve = [carla.Vector2D(0, 400), carla.Vector2D(890, phys_settings['torque1']), carla.Vector2D(5729.577, 400)] self.speed = phys_settings['speed'] # Apply Vehicle Physics Control for the vehicle self.player.apply_physics_control(physics_control)
def point_inside_boundingbox(point, bb_center, bb_extent): """ X :param point: :param bb_center: :param bb_extent: :return: """ # pylint: disable=invalid-name A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y) B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y) D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + bb_extent.y) M = carla.Vector2D(point.x, point.y) AB = B - A AD = D - A AM = M - A am_ab = AM.x * AB.x + AM.y * AB.y ab_ab = AB.x * AB.x + AB.y * AB.y am_ad = AM.x * AD.x + AM.y * AD.y ad_ad = AD.x * AD.x + AD.y * AD.y return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad
def render(self, display: pg.Surface, turn_angle: float): """ Render the indicator @param: turn_angle wheels turn angle in radians """ # background w/ border round_rect(self.surface, pg.Rect(0, 0, self.size[0], self.size[1]), self.border_color, self.radius, self.border, self.inner_color) line_width = 1 axle_width = self.size[0] - (2 * self.h_offset) frame_height = self.size[1] - (2 * self.v_offset) front_axle_y = self.v_offset rear_axle_y = self.v_offset + frame_height wheel_half_size = 18 # front axle pg.draw.line(self.surface, self.border_color, (self.h_offset, front_axle_y), (self.h_offset + axle_width, self.v_offset), line_width) # rear axle pg.draw.line(self.surface, self.border_color, (self.h_offset, rear_axle_y), (self.h_offset + axle_width, rear_axle_y), line_width) # frame center pg.draw.line(self.surface, self.border_color, (self.h_offset + axle_width // 2, front_axle_y), (self.h_offset + axle_width // 2, rear_axle_y), line_width) for x_offset in [self.h_offset, self.h_offset + axle_width]: # front wheels pivot_p = carla.Vector2D(x_offset, front_axle_y) from_p = carla.Vector2D(x_offset, front_axle_y + wheel_half_size) to_p = carla.Vector2D(x_offset, front_axle_y - wheel_half_size) from_p = self._rotate_point(pivot_p, from_p, turn_angle) to_p = self._rotate_point(pivot_p, to_p, turn_angle) pg.draw.line(self.surface, self.border_color, (from_p.x, from_p.y), (to_p.x, to_p.y), 10) # bottom wheels pg.draw.line(self.surface, self.border_color, (x_offset, rear_axle_y + wheel_half_size), (x_offset, rear_axle_y - wheel_half_size), 10) # update display display.blit(self.surface, self.pos)
def _point_inside_boundingbox(self, point, bb_center, bb_extent): A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y) B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y) D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + bb_extent.y) M = carla.Vector2D(point.x, point.y) AB = B - A AD = D - A AM = M - A am_ab = AM.x * AB.x + AM.y * AB.y ab_ab = AB.x * AB.x + AB.y * AB.y am_ad = AM.x * AD.x + AM.y * AD.y ad_ad = AD.x * AD.x + AD.y * AD.y return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad
def cb_car_info(self, car_info): self.car_info = car_info if car_info.initial: self.length = ( carla.Vector2D(car_info.front_axle_center.x, car_info.front_axle_center.y) - carla.Vector2D(car_info.rear_axle_center.x, car_info.rear_axle_center.y)).length() self.rear_length = ( carla.Vector2D(car_info.car_pos.x, car_info.car_pos.y) - carla.Vector2D(car_info.rear_axle_center.x, car_info.rear_axle_center.y)).length() self.max_steer_angle = self.car_info.max_steer_angle print('car length {}, rear length {}'.format( self.length, self.rear_length))
def publish_plan(self): current_time = rospy.Time.now() gui_path = NavPath() gui_path.header.frame_id = 'map' gui_path.header.stamp = current_time values = [(carla.Vector2D(self.actor.get_location().x, self.actor.get_location().y), self.actor.get_transform().rotation.yaw)] # Exclude last point because no yaw information. values += [(self.path.get_position(i), self.path.get_yaw(i)) for i in range(len(self.path.route_points) - 1)] for (position, yaw) in values: pose = PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = current_time pose.pose.position.x = position.x pose.pose.position.y = position.y pose.pose.position.z = 0 quaternion = tf.transformations.quaternion_from_euler( 0, 0, np.deg2rad(yaw)) pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] gui_path.poses.append(pose) self.plan_pub.publish(gui_path)
def _rotate_point(self, center: carla.Vector2D, point: carla.Vector2D, angle: float): return carla.Vector2D( math.cos(angle) * (point.x - center.x) - math.sin(angle) * (point.y - center.y) + center.x, math.sin(angle) * (point.x - center.x) + math.cos(angle) * (point.y - center.y) + center.y)
def parse_vector_list(info): """Parses a list of string into a list of Vector2D""" vector_list = [] for i in range(0, len(info), 2): vector = carla.Vector2D( x=float(info[i][1:-1]), y=float(info[i + 1][:-1]), ) vector_list.append(vector) return vector_list
def update_crowd_range(self): # Cap frequency so that GAMMA loop doesn't die. if self.last_crowd_range_update is None or time.time( ) - self.last_crowd_range_update > 1.0: pos = self.actor.get_location() bounds_min = carla.Vector2D(pos.x - self.crowd_range, pos.y - self.crowd_range) bounds_max = carla.Vector2D(pos.x + self.crowd_range, pos.y + self.crowd_range) exclude_bounds_min = carla.Vector2D( pos.x - self.exclude_crowd_range, pos.y - self.exclude_crowd_range) exclude_bounds_max = carla.Vector2D( pos.x + self.exclude_crowd_range, pos.y + self.exclude_crowd_range) self.crowd_service.simulation_bounds = (bounds_min, bounds_max) self.crowd_service.forbidden_bounds = (exclude_bounds_min, exclude_bounds_max) self.last_crowd_range_update = time.time()
def __init__(self): address = rospy.get_param('address', '127.0.0.1') port = rospy.get_param('port', 2000) self.map_location = rospy.get_param('map_location', 'meskel_square') self.random_seed = rospy.get_param('random_seed', 1) self.rng = random.Random(rospy.get_param('random_seed', 0)) sys.stdout.flush() with (DATA_PATH / '{}.sim_bounds'.format(self.map_location)).open('r') as f: bounds_min = carla.Vector2D( *[float(v) for v in f.readline().split(',')]) bounds_max = carla.Vector2D( *[float(v) for v in f.readline().split(',')]) self.bounds_occupancy = carla.OccupancyMap(bounds_min, bounds_max) sys.stdout.flush() self.sumo_network = carla.SumoNetwork.load( str(DATA_PATH / '{}.net.xml'.format(self.map_location))) self.sumo_network_segments = self.sumo_network.create_segment_map() self.sumo_network_spawn_segments = self.sumo_network_segments.intersection( carla.OccupancyMap(bounds_min, bounds_max)) self.sumo_network_spawn_segments.seed_rand(self.rng.getrandbits(32)) self.sumo_network_occupancy = carla.OccupancyMap.load( str(DATA_PATH / '{}.network.wkt'.format(self.map_location))) sys.stdout.flush() self.sidewalk = self.sumo_network_occupancy.create_sidewalk(1.5) self.sidewalk_segments = self.sidewalk.create_segment_map() self.sidewalk_spawn_segments = self.sidewalk_segments.intersection( carla.OccupancyMap(bounds_min, bounds_max)) self.sidewalk_spawn_segments.seed_rand(self.rng.getrandbits(32)) self.sidewalk_occupancy = carla.OccupancyMap.load( str(DATA_PATH / '{}.sidewalk.wkt'.format(self.map_location))) sys.stdout.flush() self.client = carla.Client(address, port) self.client.set_timeout(10.0) self.world = self.client.get_world() sys.stdout.flush()
def main(): # Connect to client client = carla.Client('127.0.0.1', 2000) client.set_timeout(2.0) # Get World and Actors world = client.get_world() current_map = world.get_map() actors = world.get_actors() # Get a random vehicle from world (there should be one at least) vehicle = random.choice([actor for actor in actors if 'vehicle' in actor.type_id]) # Create Wheels Physics Control front_left_wheel = carla.WheelPhysicsControl(tire_friction=4.5, damping_rate=1.0, max_steer_angle=70.0, radius=30.0) front_right_wheel = carla.WheelPhysicsControl(tire_friction=2.5, damping_rate=1.5, max_steer_angle=70.0, radius=25.0) rear_left_wheel = carla.WheelPhysicsControl(tire_friction=1.0, damping_rate=0.2, max_steer_angle=0.0, radius=15.0) rear_right_wheel = carla.WheelPhysicsControl(tire_friction=1.5, damping_rate=1.3, max_steer_angle=0.0, radius=20.0) wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel] # Change Vehicle Physics Control parameters of the vehicle physics_control = vehicle.get_physics_control() physics_control.torque_curve = [carla.Vector2D(x=0, y=400), carla.Vector2D(x=1300, y=600)] physics_control.max_rpm = 100000 physics_control.moi = 1.0 physics_control.damping_rate_full_throttle = 0.0 physics_control.use_gear_autobox = True physics_control.gear_switch_time = 0.5 physics_control.clutch_strength = 10 physics_control.mass = 10000 physics_control.drag_coefficient = 0.25 physics_control.steering_curve = [carla.Vector2D(x=0, y=1), carla.Vector2D(x=100, y=1), carla.Vector2D(x=300, y=1)] physics_control.wheels = wheels physics_control.use_sweep_wheel_collision = True # Apply Vehicle Physics Control for the vehicle vehicle.apply_physics_control(physics_control)
def publish_il_car_info(self, step=None): car_info_msg = CarInfo() pos = self.actor.get_location() pos2D = carla.Vector2D(pos.x, pos.y) vel = self.actor.get_velocity() yaw = np.deg2rad(self.actor.get_transform().rotation.yaw) v_2d = np.array([vel.x, vel.y, 0]) forward = np.array([math.cos(yaw), math.sin(yaw), 0]) speed = np.vdot(forward, v_2d) self.speed = speed car_info_msg.id = self.actor.id car_info_msg.car_pos.x = pos.x car_info_msg.car_pos.y = pos.y car_info_msg.car_pos.z = pos.z car_info_msg.car_yaw = yaw car_info_msg.car_speed = speed car_info_msg.car_steer = self.actor.get_control().steer car_info_msg.car_vel.x = vel.x car_info_msg.car_vel.y = vel.y car_info_msg.car_vel.z = vel.z car_info_msg.car_bbox = Polygon() corners = get_bounding_box_corners(self.actor) for corner in corners: car_info_msg.car_bbox.points.append( Point32(x=corner.x, y=corner.y, z=0.0)) car_info_msg.initial = False if step is None: wheels = self.actor.get_physics_control().wheels # TODO I think that CARLA might have forgotten to divide by 100 here. wheel_positions = [w.position / 100 for w in wheels] front_axle_center = (wheel_positions[0] + wheel_positions[1]) / 2 rear_axle_center = (wheel_positions[2] + wheel_positions[3]) / 2 car_info_msg.front_axle_center.x = front_axle_center.x car_info_msg.front_axle_center.y = front_axle_center.y car_info_msg.front_axle_center.z = front_axle_center.z car_info_msg.rear_axle_center.x = rear_axle_center.x car_info_msg.rear_axle_center.y = rear_axle_center.y car_info_msg.rear_axle_center.z = rear_axle_center.z car_info_msg.max_steer_angle = wheels[0].max_steer_angle car_info_msg.initial = True try: self.car_info_pub.publish(car_info_msg) except Exception as e: print(e)
def update_path(self, lane_decision): if lane_decision == REMAIN: return pos = self.actor.get_location() ego_veh_pos = carla.Vector2D(pos.x, pos.y) yaw = np.deg2rad(self.actor.get_transform().rotation.yaw) forward_vec = carla.Vector2D(math.cos(yaw), math.sin(yaw)) sidewalk_vec = forward_vec.rotate( np.deg2rad(90)) # rotate clockwise by 90 degree ego_veh_pos_in_new_lane = None if lane_decision == CHANGE_LEFT: ego_veh_pos_in_new_lane = ego_veh_pos - 4.0 * sidewalk_vec else: ego_veh_pos_in_new_lane = ego_veh_pos + 4.0 * sidewalk_vec cur_route_point = self.sumo_network.get_nearest_route_point( self.path.get_position(0)) new_route_point = self.sumo_network.get_nearest_route_point( ego_veh_pos_in_new_lane) lane_change_probability = 1.0 if new_route_point.edge == cur_route_point.edge and new_route_point.lane != cur_route_point.lane: if self.rng.uniform(0.0, 1.0) <= lane_change_probability: new_path_candidates = self.sumo_network.get_next_route_paths( new_route_point, self.path.min_points - 1, self.path.interval) new_path = NetworkAgentPath(self.sumo_network, self.path.min_points, self.path.interval) new_path.route_points = self.rng.choice( new_path_candidates)[0:self.path.min_points] print('NEW PATH!') sys.stdout.flush() self.path = new_path
def get_bounding_box_corners(actor): bbox = actor.bounding_box loc = carla.Vector2D(bbox.location.x, bbox.location.y) + get_position(actor) forward_vec = get_forward_direction(actor).make_unit_vector() sideward_vec = forward_vec.rotate(np.deg2rad(90)) half_y_len = bbox.extent.y half_x_len = bbox.extent.x corners = [ loc - half_x_len * forward_vec + half_y_len * sideward_vec, loc + half_x_len * forward_vec + half_y_len * sideward_vec, loc + half_x_len * forward_vec - half_y_len * sideward_vec, loc - half_x_len * forward_vec - half_y_len * sideward_vec ] return corners
def get_pedestrian_bounding_box_corners(actor): bbox = actor.bounding_box loc = carla.Vector2D(bbox.location.x, bbox.location.y) + get_position(actor) forward_vec = get_forward_direction(actor).make_unit_vector() sideward_vec = forward_vec.rotate(np.deg2rad(90)) # Hardcoded values for pedestrians. half_y_len = 0.25 half_x_len = 0.25 corners = [ loc - half_x_len * forward_vec + half_y_len * sideward_vec, loc + half_x_len * forward_vec + half_y_len * sideward_vec, loc + half_x_len * forward_vec - half_y_len * sideward_vec, loc - half_x_len * forward_vec - half_y_len * sideward_vec ] return corners
def parse_vector_list(info): """ Parses a list of string into a list of Vector2D Args: info (list): list corresponding to a row of the recorder """ vector_list = [] for i in range(0, len(info), 2): vector = carla.Vector2D( float(info[i][1:-1]), float(info[i + 1][:-1]), ) vector_list.append(vector) return vector_list
REMAIN = 0 CHANGE_RIGHT = 1 VEHICLE_STEER_KP = 1.5 # 2.0 PID_TUNING_ON = False Pyro4.config.SERIALIZERS_ACCEPTED.add('serpent') Pyro4.config.SERIALIZER = 'serpent' Pyro4.util.SerializerBase.register_class_to_dict( carla.Vector2D, lambda o: { '__class__': 'carla.Vector2D', 'x': o.x, 'y': o.y }) Pyro4.util.SerializerBase.register_dict_to_class( 'carla.Vector2D', lambda c, o: carla.Vector2D(o['x'], o['y'])) ''' ========== UTILITY FUNCTIONS AND CLASSES ========== ''' class NetworkAgentPath: def __init__(self, sumo_network, min_points, interval): self.sumo_network = sumo_network self.min_points = min_points self.interval = interval self.route_points = [] @staticmethod def rand_path(sumo_network, min_points, interval, segment_map,
def project(a): # LatLon -> CARLA coordinates. return carla.Vector2D(lat2y(a[0]), lon2x(a[1]))
def main(): actor_list = [] try: client = carla.Client('localhost', 2000) client.set_timeout(2.0) new_map = 'Town03' print("Changing map to {}".format(new_map)) world = client.load_world(new_map) world.set_weather(carla.WeatherParameters(cloudyness=20.0, sun_azimuth_angle=90, sun_altitude_angle=90)) settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = 0.1 world.apply_settings(settings) spectator = world.get_spectator() # the spectator is the view of the simulator window blueprint_library = world.get_blueprint_library() bp = blueprint_library.find('vehicle.audi.tt') color = '156,52,8' bp.set_attribute('color', color) # get the map of the world and the waypoint for the starting location world_map = world.get_map() starting_loc = ROUTE[0] starting_wpt = world_map.get_waypoint(carla.Location(x=starting_loc[0], y=starting_loc[1], z=starting_loc[2])) # spawn the vehicle at the chosen waypoint and add it to the actor list vehicle = world.spawn_actor(bp, starting_wpt.transform) actor_list.append(vehicle) print('created %s' % vehicle.type_id) vd = VehicleData() lidar = add_lidar_sensor(world, vehicle, vd) actor_list.append(lidar) # vehicle.set_autopilot(True) # move simulation view to center on vehicle, up high world.tick() world_snapshot = world.get_snapshot() actor_snapshot = world_snapshot.find(vehicle.id) spectator_transform = actor_snapshot.get_transform() spectator_transform.location += carla.Location(x=0, y=0, z=100.0) spectator_transform.rotation.pitch = -89 spectator_transform.rotation.yaw = -179 spectator_transform.rotation.roll = -1 spectator.set_transform(spectator_transform) pc = vehicle.get_physics_control(); # get rid of the steering curve pc.steering_curve = [carla.Vector2D(0.0, 1.0), carla.Vector2D(120.0, 1.0)]; vehicle.apply_physics_control(pc) # wheel positions are in global coordinates and in centimeters, convert to wheelbase wheel_xy = np.array([(pc.wheels[0].position.x - pc.wheels[2].position.x), (pc.wheels[0].position.y - pc.wheels[2].position.y)]) wheelbase = 1e-2*np.linalg.norm(wheel_xy) # keep track of Vehicle State information max_steer = pc.wheels[0].max_steer_angle vd.config(wheelbase, max_steer) # create a basic agent of the vehicle agent = BasicAgent(vehicle, target_speed=40) agent.set_destination_list(ROUTE) # drive to waypoints until they are all gone while len(agent._local_planner._waypoints_queue) > 0: # for i in range(150): # print(i) # world.wait_for_tick(10.00) world.tick() snapshot = world.get_snapshot() time = snapshot.elapsed_seconds control = agent.run_step() control.manual_gear_shift = False vehicle.apply_control(control) vd.appendTime(time) vd.appendVelocityData(vehicle.get_velocity()) vd.appendControlData(control) vd.appendTransformTruth(vehicle.get_transform()) control.brake = 1.0 control.throttle = 0.0 vehicle.apply_control(control) world.tick() if args.plot: vd.runMotionModelFull() vd.plot() if args.vehicle_data_file is not None: vd.saveToFile(args.vehicle_data_file) print("Saved to {}".format(args.vehicle_data_file)) print('Finished following waypoints') # END - Take the simulation out of synchronous/fixed time mode settings = world.get_settings() settings.synchronous_mode = False settings.fixed_delta_seconds = None world.apply_settings(settings) if args.interact: import code code.interact(local=locals()) finally: print('destroying actors') for actor in actor_list: if actor.is_alive: actor.destroy() print('done.')
def get_heading(actor): heading = actor.get_transform().get_forward_vector() return carla.Vector2D(heading.x, heading.y)
def main(): actor_list = [] try: #connect to server client = carla.Client('localhost', 2000) client.set_timeout(2.0) #load sumo network sumo_network = carla.SumoNetwork.load(str(DATA_PATH/'{}.net.xml'.format("meskel_square"))) world = client.get_world() # Get arbitrary position. position = carla.Vector2D(350, 350) # Get nearest route point on SUMO network. route_point = sumo_network.get_nearest_route_point(position) # Get route point position. route_point_position = sumo_network.get_route_point_position(route_point) print("route_point_position: ",route_point_position) # Get route point transform route_point_transform = carla.Transform(carla.Location(x=route_point_position.x,y=route_point_position.y,z=0.5)) print("route_point_transform: ",route_point_transform) blueprint_library = world.get_blueprint_library() #choose the vehicle you want bp = blueprint_library.filter("model3")[0] print(bp) print("spawn_point: ",route_point_transform) vehicle = world.spawn_actor(bp,route_point_transform) #let vechicle move #vehicle.apply_control(carla.VehicleControl(throttle=0.5,steer=0.0)) actor_list.append(vehicle) #camera camera_bp = blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute("image_size_x",str(IM_WIDTH)) camera_bp.set_attribute("image_size_y",str(IM_HEIGHT)) camera_bp.set_attribute("fov","120") #relative to center of vehicle (left hand coordination) spawn_point = carla.Transform(carla.Location(x=-5,y=3,z=5),carla.Rotation(pitch=-40,yaw=-50)) #attach sensor to vehicle camera = world.spawn_actor(camera_bp, spawn_point, attach_to=vehicle) actor_list.append(camera) camera.listen(lambda data : process_img(data)) time.sleep(20) finally: print('destroying actors') for actor in actor_list: #print(actor) actor.destroy() print('done.')
def test_named_args(self): torque_curve = [[0, 400], [24, 56], [24, 56], [1315.47, 654.445], [5729, 400]] steering_curve = [ carla.Vector2D(x=0, y=1), carla.Vector2D(x=20.0, y=0.9), carla.Vector2D(x=63.0868, y=0.703473), carla.Vector2D(x=119.12, y=0.573047) ] wheels = [ carla.WheelPhysicsControl(tire_friction=2, damping_rate=0, steer_angle=30, disable_steering=1), carla.WheelPhysicsControl(tire_friction=2, damping_rate=0, steer_angle=30, disable_steering=1), carla.WheelPhysicsControl(tire_friction=2, damping_rate=0, steer_angle=30, disable_steering=1), carla.WheelPhysicsControl(tire_friction=2, damping_rate=0, steer_angle=30, disable_steering=1) ] pc = carla.VehiclePhysicsControl( torque_curve=torque_curve, max_rpm=5729, moi=1, damping_rate_full_throttle=0.15, damping_rate_zero_throttle_clutch_engaged=2, damping_rate_zero_throttle_clutch_disengaged=0.35, use_gear_autobox=1, gear_switch_time=0.5, clutch_strength=10, mass=5500, drag_coefficient=0.3, center_of_mass=carla.Vector3D(x=0.5, y=1, z=1), steering_curve=steering_curve, wheels=wheels) error = .001 for i in range(0, len(torque_curve)): self.assertTrue( abs(pc.torque_curve[i].x - torque_curve[i][0]) <= error) self.assertTrue( abs(pc.torque_curve[i].y - torque_curve[i][1]) <= error) self.assertTrue(abs(pc.max_rpm - 5729) <= error) self.assertTrue(abs(pc.moi - 1) <= error) self.assertTrue(abs(pc.damping_rate_full_throttle - 0.15) <= error) self.assertTrue( abs(pc.damping_rate_zero_throttle_clutch_engaged - 2) <= error) self.assertTrue( abs(pc.damping_rate_zero_throttle_clutch_disengaged - 0.35) <= error) self.assertTrue(abs(pc.use_gear_autobox - 1) <= error) self.assertTrue(abs(pc.gear_switch_time - 0.5) <= error) self.assertTrue(abs(pc.clutch_strength - 10) <= error) self.assertTrue(abs(pc.mass - 5500) <= error) self.assertTrue(abs(pc.drag_coefficient - 0.3) <= error) self.assertTrue(abs(pc.center_of_mass.x - 0.5) <= error) self.assertTrue(abs(pc.center_of_mass.y - 1) <= error) self.assertTrue(abs(pc.center_of_mass.z - 1) <= error) for i in range(0, len(steering_curve)): self.assertTrue( abs(pc.steering_curve[i].x - steering_curve[i].x) <= error) self.assertTrue( abs(pc.steering_curve[i].y - steering_curve[i].y) <= error) for i in range(0, len(wheels)): self.assertTrue( abs(pc.wheels[i].tire_friction - wheels[i].tire_friction) <= error) self.assertTrue( abs(pc.wheels[i].damping_rate - wheels[i].damping_rate) <= error) self.assertTrue( abs(pc.wheels[i].steer_angle - wheels[i].steer_angle) <= error) self.assertEqual(pc.wheels[i].disable_steering, wheels[i].disable_steering)
def get_velocity(actor): v = actor.get_velocity() return carla.Vector2D(v.x, v.y)
def get_position(actor): pos3d = actor.get_location() return carla.Vector2D(pos3d.x, pos3d.y)
def main(args): # Load context data. with (DATA_PATH / '{}.sim_bounds'.format(DATASET)).open('r') as f: bounds_min = carla.Vector2D( *[float(v) for v in f.readline().split(',')]) bounds_max = carla.Vector2D( *[float(v) for v in f.readline().split(',')]) bounds_occupancy = carla.OccupancyMap(bounds_min, bounds_max) sumo_network = carla.SumoNetwork.load( str(DATA_PATH / '{}.net.xml'.format(DATASET))) sumo_network_segments = sumo_network.create_segment_map() sumo_network_spawn_segments = sumo_network_segments.intersection( carla.OccupancyMap(bounds_min, bounds_max)) sumo_network_occupancy = carla.OccupancyMap.load( str(DATA_PATH / '{}.network.wkt'.format(DATASET))) sidewalk = sumo_network_occupancy.create_sidewalk(1.5) # Connect to simulator. client = carla.Client(args.host, args.port) client.set_timeout(10.0) world = client.get_world() # Generate path. spawn_point = sumo_network.get_nearest_route_point(SPAWN_POSITION) ego_path = SumoNetworkAgentPath([spawn_point], 200, 1.0) ego_path.resize(sumo_network) # Calculate transform for spawning. spawn_position = ego_path.get_position(sumo_network, 0) spawn_heading_yaw = ego_path.get_yaw(sumo_network, 0) spawn_transform = carla.Transform() spawn_transform.location.x = spawn_position.x spawn_transform.location.y = spawn_position.y spawn_transform.location.z = 1.0 spawn_transform.rotation.yaw = spawn_heading_yaw # Spawn ego actor and spectator camera. ego_actor = world.spawn_actor( world.get_blueprint_library().filter('vehicle.audi.etron')[0], spawn_transform) # Main control loop. while True: # Prepare GAMMA instance. gamma = carla.RVOSimulator() # Add exo-agents to GAMMA. gamma_id = 0 for actor in world.get_actors(): if actor.id != ego_actor.id: if isinstance(actor, carla.Vehicle): if is_bike(actor): type_tag = 'Bicycle' else: type_tag = 'Car' bounding_box_corners = get_vehicle_bounding_box_corners( actor) elif isinstance(actor, carla.Walker): type_tag = 'People' bounding_box_corners = get_pedestrian_bounding_box_corners( actor) else: continue gamma.add_agent(carla.AgentParams.get_default(type_tag), gamma_id) gamma.set_agent_position(gamma_id, get_position(actor)) gamma.set_agent_velocity(gamma_id, get_velocity(actor)) gamma.set_agent_heading(gamma_id, get_forward_direction(actor)) gamma.set_agent_bounding_box_corners(gamma_id, bounding_box_corners) gamma.set_agent_pref_velocity(gamma_id, get_velocity(actor)) gamma_id += 1 # Extend ego path. ego_path.cut(sumo_network, get_position(ego_actor)) ego_path.resize(sumo_network) # Add ego-agent to GAMMA. ego_id = gamma_id gamma.add_agent(carla.AgentParams.get_default('Car'), ego_id) gamma.set_agent_position(ego_id, get_position(ego_actor)) gamma.set_agent_velocity(ego_id, get_velocity(ego_actor)) gamma.set_agent_heading(ego_id, get_forward_direction(ego_actor)) gamma.set_agent_bounding_box_corners( ego_id, get_vehicle_bounding_box_corners(ego_actor)) target_position = ego_path.get_position(sumo_network, 5) pref_vel = MAX_SPEED * (target_position - get_position(ego_actor)).make_unit_vector() path_forward = ( ego_path.get_position(sumo_network, 1) - ego_path.get_position(sumo_network, 0)).make_unit_vector() gamma.set_agent_pref_velocity(ego_id, pref_vel) gamma.set_agent_path_forward(ego_id, path_forward) left_line_end = get_position(ego_actor) + (1.5 + 2.0 + 0.8) * ( (get_forward_direction(ego_actor).rotate( np.deg2rad(-90))).make_unit_vector()) right_line_end = get_position(ego_actor) + (1.5 + 2.0 + 0.8) * ( (get_forward_direction(ego_actor).rotate( np.deg2rad(90))).make_unit_vector()) left_lane_constrained_by_sidewalk = sidewalk.intersects( carla.Segment2D(get_position(ego_actor), left_line_end)) right_lane_constrained_by_sidewalk = sidewalk.intersects( carla.Segment2D(get_position(ego_actor), right_line_end)) # Flip left-right -> right-left since GAMMA uses a different handed coordinate system. gamma.set_agent_lane_constraints(ego_id, right_lane_constrained_by_sidewalk, left_lane_constrained_by_sidewalk) # Step GAMMA, and retrieve ego-agent's target velocity. gamma.do_step() target_vel = gamma.get_agent_velocity(ego_id) # Calculate ego-vehicle control. control = carla.VehicleControl() control.throttle = KP_THROTTLE * (target_vel.length() - get_velocity(ego_actor).length()) control.steer = KP_STEERING * get_signed_angle_diff( target_vel, get_velocity(ego_actor)) control.manual_gear_shift = True # Reduces transmission lag. control.gear = 1 # Reduces transmission lag. ego_actor.apply_control(control) time.sleep(0.1)
def array_to_vector2D(array): vector = carla.Vector2D(array[0], array[1]) return vector
def update_gamma_lane_decision(self): if not self.actor: return pos = self.actor.get_location() pos2D = carla.Vector2D(pos.x, pos.y) vel = self.actor.get_velocity() yaw = np.deg2rad(self.actor.get_transform().rotation.yaw) forward_vec = carla.Vector2D(math.cos(yaw), math.sin(yaw)) sidewalk_vec = forward_vec.rotate( np.deg2rad(90)) # rotate clockwise by 90 degree ego_veh_pos = pos2D left_ego_veh_pos = ego_veh_pos - 4.0 * sidewalk_vec right_ego_veh_pos = ego_veh_pos + 4.0 * sidewalk_vec cur_route_point = self.sumo_network.get_nearest_route_point( ego_veh_pos) left_route_point = self.sumo_network.get_nearest_route_point( left_ego_veh_pos) right_route_point = self.sumo_network.get_nearest_route_point( right_ego_veh_pos) left_lane_exist = False right_lane_exist = False if left_route_point.edge == cur_route_point.edge and left_route_point.lane != cur_route_point.lane: left_lane_exist = True else: left_lane_exist = False if right_route_point.edge == cur_route_point.edge and right_route_point.lane != cur_route_point.lane: right_lane_exist = True else: right_lane_exist = False min_dist_to_front_veh = self.dist_to_nearest_agt_in_region( ego_veh_pos, forward_vec, sidewalk_vec, lookahead_x=30, lookahead_y=4, ref_point=None) min_dist_to_left_front_veh = -1.0 min_dist_to_right_front_veh = -1.0 if left_lane_exist: # if want to change lane, also need to consider vehicles behind min_dist_to_left_front_veh = self.dist_to_nearest_agt_in_region( left_ego_veh_pos, forward_vec, sidewalk_vec, lookahead_x=35, lookahead_y=4, ref_point=left_ego_veh_pos - 12.0 * forward_vec, consider_ped=True) if right_lane_exist: # if want to change lane, also need to consider vehicles behind min_dist_to_right_front_veh = self.dist_to_nearest_agt_in_region( right_ego_veh_pos, forward_vec, sidewalk_vec, lookahead_x=35, lookahead_y=4, ref_point=right_ego_veh_pos - 12.0 * forward_vec, consider_ped=True) lane_decision = None if min_dist_to_front_veh >= 15: lane_decision = REMAIN else: if min_dist_to_left_front_veh > min_dist_to_right_front_veh: if min_dist_to_left_front_veh > min_dist_to_front_veh + 3.0: lane_decision = CHANGE_LEFT else: lane_decision = REMAIN else: # min_dist_to_left_front_veh <= min_dist_to_right_front_veh: if min_dist_to_right_front_veh > min_dist_to_front_veh + 3.0: lane_decision = CHANGE_RIGHT else: lane_decision = REMAIN if lane_decision != self.last_decision and lane_decision * self.last_decision != -1: self.update_path(lane_decision) self.last_decision = lane_decision
def get_forward_direction(actor): forward = actor.get_transform().get_forward_vector() return carla.Vector2D(forward.x, forward.y)
def update(self): end_time = rospy.Time.now() elapsed = (end_time - init_time).to_sec() if not self.ego_car_info: return if len(self.world.get_actors()) > self.total_num_agents / 1.2 or time.time() -start_time > 15.0: # print("[crowd_processor.py] {} crowd agents ready".format( # len(self.world.get_actors()))) self.agents_ready_pub.publish(True) else: pass # print("[crowd_processor.py] {} percent of agents ready".format( # len(self.world.get_actors()) / float(self.total_num_agents))) ego_car_position = self.ego_car_info.car_pos ego_car_position = carla.Vector2D( ego_car_position.x, ego_car_position.y) agents_msg = msg_builder.msg.TrafficAgentArray() agents_path_msg = msg_builder.msg.AgentPathArray() current_time = rospy.Time.now() self.crowd_service.acquire_local_intentions() local_intentions = self.crowd_service.local_intentions self.crowd_service.release_local_intentions() local_intentions_lookup = {} for x in local_intentions: local_intentions_lookup[x[0]] = x[1:] for actor in self.world.get_actors(): if not actor.id in local_intentions_lookup: continue if actor is None: continue local_intention = local_intentions_lookup[actor.id] actor_pos = carla.Vector2D(actor.get_location().x, actor.get_location().y) if (actor_pos - ego_car_position).length() > 50: # TODO Add as ROS parameter. continue agent_tmp = msg_builder.msg.TrafficAgent() agent_tmp.last_update = current_time agent_tmp.id = actor.id agent_tmp.type = {'Car': 'car', 'Bicycle': 'bike', 'People': 'ped'}[local_intention[0]] agent_tmp.pose.position.x = actor.get_location().x agent_tmp.pose.position.y = actor.get_location().y agent_tmp.pose.position.z = actor.get_location().z agent_tmp.vel.x = actor.get_velocity().x agent_tmp.vel.y = actor.get_velocity().y quat_tf = tf.transformations.quaternion_from_euler( 0, 0, np.deg2rad(actor.get_transform().rotation.yaw)) agent_tmp.pose.orientation = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3]) agent_tmp.bbox = Polygon() corners = get_bounding_box_corners(actor, expand=0.3) for corner in corners: agent_tmp.bbox.points.append(Point32( x=corner.x, y=corner.y, z=0.0)) agents_msg.agents.append(agent_tmp) agent_paths_tmp = msg_builder.msg.AgentPaths() agent_paths_tmp.id = actor.id agent_paths_tmp.type = {'Car': 'car', 'Bicycle': 'bike', 'People': 'ped'}[local_intention[0]] if local_intention[0] in ['Car', 'Bicycle']: initial_route_point = carla.SumoNetworkRoutePoint() initial_route_point.edge = local_intention[1].edge initial_route_point.lane = local_intention[1].lane initial_route_point.segment = local_intention[1].segment initial_route_point.offset = local_intention[1].offset paths = [[initial_route_point]] topological_hash = '' # TODO Add 20, 1.0 as ROS paramters. for _ in range(20): next_paths = [] for path in paths: next_route_points = self.sumo_network.get_next_route_points(path[-1], 1.0) next_paths.extend(path + [route_point] for route_point in next_route_points) if len(next_route_points) > 1: topological_hash += '({},{},{},{}={})'.format( path[-1].edge, path[-1].lane, path[-1].segment, path[-1].offset, len(next_route_points)) paths = next_paths agent_paths_tmp.reset_intention = self.topological_hash_map[actor.id] is None or \ self.topological_hash_map[actor.id] != topological_hash for path in paths: path_msg = Path() path_msg.header.frame_id = 'map' path_msg.header.stamp = current_time for path_point in [actor_pos] + \ [self.sumo_network.get_route_point_position(route_point) for route_point in path]: pose_msg = PoseStamped() pose_msg.header.frame_id = 'map' pose_msg.header.stamp = current_time pose_msg.pose.position.x = path_point.x pose_msg.pose.position.y = path_point.y path_msg.poses.append(pose_msg) # self.draw_path(path_msg) agent_paths_tmp.path_candidates.append(path_msg) agent_paths_tmp.cross_dirs = [] self.topological_hash_map[actor.id] = topological_hash elif local_intention[0] == 'People': sidewalk_route_point = carla.SidewalkRoutePoint() sidewalk_route_point.polygon_id = local_intention[1].polygon_id sidewalk_route_point.segment_id = local_intention[1].segment_id sidewalk_route_point.offset = local_intention[1].offset sidewalk_route_orientation = local_intention[2] path = [sidewalk_route_point] for _ in range(20): if sidewalk_route_orientation: path.append(self.sidewalk.get_next_route_point(path[-1], 1.0)) # TODO Add as ROS parameter. else: path.append(self.sidewalk.get_previous_route_point(path[-1], 1.0)) # TODO Add as ROS parameter. topological_hash = '{},{}'.format(sidewalk_route_point.polygon_id, sidewalk_route_orientation) agent_paths_tmp.reset_intention = self.topological_hash_map[actor.id] is None or \ self.topological_hash_map[actor.id] != topological_hash path_msg = Path() path_msg.header.frame_id = 'map' path_msg.header.stamp = current_time for path_point in [actor_pos] + [self.sidewalk.get_route_point_position(route_point) for route_point in path]: pose_msg = PoseStamped() pose_msg.header.frame_id = 'map' pose_msg.header.stamp = current_time pose_msg.pose.position.x = path_point.x pose_msg.pose.position.y = path_point.y path_msg.poses.append(pose_msg) # self.draw_path(path_msg) agent_paths_tmp.path_candidates = [path_msg] agent_paths_tmp.cross_dirs = [sidewalk_route_orientation] self.topological_hash_map[actor.id] = topological_hash agents_path_msg.agents.append(agent_paths_tmp) try: agents_msg.header.frame_id = 'map' agents_msg.header.stamp = current_time self.agents_pub.publish(agents_msg) agents_path_msg.header.frame_id = 'map' agents_path_msg.header.stamp = current_time self.agents_path_pub.publish(agents_path_msg) except Exception as e: print(e) # self.do_update = False end_time = rospy.Time.now() elapsed = (end_time - init_time).to_sec()
def get_position(self): location = self.actor.get_location() if location.z < -0.3: print("Car dropped under ground") self.ego_dead_pub.publish(True) return carla.Vector2D(location.x, location.y)