def get_distance_between_actors(current, target, distance_type="euclidianDistance", freespace=False, global_planner=None): """ This function finds the distance between actors for different use cases described by distance_type and freespace attributes """ target_transform = CarlaDataProvider.get_transform(target) current_transform = CarlaDataProvider.get_transform(current) target_wp = CarlaDataProvider.get_map().get_waypoint( target_transform.location) current_wp = CarlaDataProvider.get_map().get_waypoint( current_transform.location) extent_sum_x, extent_sum_y = 0, 0 if freespace: if isinstance(target, (carla.Vehicle, carla.Walker)): extent_sum_x = target.bounding_box.extent.x + current.bounding_box.extent.x extent_sum_y = target.bounding_box.extent.y + current.bounding_box.extent.y if distance_type == "longitudinal": if not current_wp.road_id == target_wp.road_id: distance = 0 # Get the route route = global_planner.trace_route(current_transform.location, target_transform.location) # Get the distance of the route for i in range(1, len(route)): curr_loc = route[i][0].transform.location prev_loc = route[i - 1][0].transform.location distance += curr_loc.distance(prev_loc) else: distance = abs(current_wp.s - target_wp.s) if freespace: distance = distance - extent_sum_x elif distance_type == "lateral": target_t = get_troad_from_transform(target_transform) current_t = get_troad_from_transform(current_transform) distance = abs(target_t - current_t) if freespace: distance = distance - extent_sum_y elif distance_type in ["cartesianDistance", "euclidianDistance"]: distance = target_transform.location.distance( current_transform.location) if freespace: distance = distance - extent_sum_x else: raise TypeError("unknown distance_type: {}".format(distance_type)) # distance will be negative for feeespace when there is overlap condition # truncate to 0.0 when this happens distance = 0.0 if distance < 0.0 else distance return distance
def initialise(self): # In case of walkers, we have to extract the current heading if self._type == 'walker': self._control.speed = self._target_velocity self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() super(AccelerateToVelocity, self).initialise()
def initialise(self): self._location = CarlaDataProvider.get_location(self._actor) self._start_time = GameTime.get_time() # In case of walkers, we have to extract the current heading if self._type == 'walker': self._control.speed = self._target_velocity self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() super(KeepVelocity, self).initialise()
def _set_new_velocity(self, next_location): """ Calculate and set the new actor veloctiy given the current actor location and the _next_location_ Args: next_location (carla.Location): Next target location of the actor returns: direction (carla.Vector3D): Normalized direction vector of the actor """ # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) direction_norm = math.sqrt(direction.x**2 + direction.y**2) velocity.x = direction.x / direction_norm * self._target_speed velocity.y = direction.y / direction_norm * self._target_speed self._actor.set_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw new_yaw = CarlaDataProvider.get_map().get_waypoint( next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 angular_velocity = carla.Vector3D(0, 0, 0) angular_velocity.z = delta_yaw / (direction_norm / self._target_speed) self._actor.set_angular_velocity(angular_velocity) return direction_norm
def _set_new_velocity(self, next_location): """ Calculate and set the new actor veloctiy given the current actor location and the _next_location_ If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. If _consider_trafficlights is true, the vehicle will enforce a stop at a red traffic light. If _max_deceleration is set, the vehicle will reduce its speed according to the given deceleration value. If the vehicle reduces its speed, braking lights will be activated. Args: next_location (carla.Location): Next target location of the actor returns: direction (carla.Vector3D): Length of direction vector of the actor """ current_time = GameTime.get_time() target_speed = self._target_speed if not self._last_update: self._last_update = current_time current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity if self._obstacle_distance < self._proximity_threshold: distance = max(self._obstacle_distance, 0) if distance > 0: current_speed_other = math.sqrt( self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) if current_speed_other < current_speed: acceleration = -0.5 * ( current_speed - current_speed_other)**2 / distance target_speed = max( acceleration * (current_time - self._last_update) + current_speed, 0) else: target_speed = 0 if self._consider_traffic_lights: if (self._actor.is_at_traffic_light() and self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): target_speed = 0 if target_speed < current_speed: self._actor.set_light_state(carla.VehicleLightState.Brake) if self._max_deceleration is not None: target_speed = max( target_speed, current_speed - (current_time - self._last_update) * self._max_deceleration) else: self._actor.set_light_state(carla.VehicleLightState.NONE) if self._max_acceleration is not None: target_speed = min( target_speed, current_speed + (current_time - self._last_update) * self._max_acceleration) # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) direction_norm = math.sqrt(direction.x**2 + direction.y**2) velocity.x = direction.x / direction_norm * target_speed velocity.y = direction.y / direction_norm * target_speed self._actor.set_target_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw # When we have a waypoint list, use the direction between the waypoints to calculate the heading (change) # otherwise use the waypoint heading directly if self._waypoints: delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw else: new_yaw = CarlaDataProvider.get_map().get_waypoint( next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: angular_velocity.z = 0 else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) self._actor.set_target_angular_velocity(angular_velocity) self._last_update = current_time return direction_norm
def _set_new_velocity(self, next_location): """ Calculate and set the new actor veloctiy given the current actor location and the _next_location_ If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. Args: next_location (carla.Location): Next target location of the actor returns: direction (carla.Vector3D): Length of direction vector of the actor """ current_time = GameTime.get_time() target_speed = self._target_speed if not self._last_update: self._last_update = current_time if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity if self._obstacle_distance < self._proximity_threshold: distance = max(self._obstacle_distance, 0) if distance > 0: current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) current_speed_other = math.sqrt( self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) if current_speed_other < current_speed: acceleration = -0.5 * ( current_speed - current_speed_other)**2 / distance target_speed = max( acceleration * (current_time - self._last_update) + current_speed, 0) else: target_speed = 0 # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) direction_norm = math.sqrt(direction.x**2 + direction.y**2) velocity.x = direction.x / direction_norm * target_speed velocity.y = direction.y / direction_norm * target_speed self._actor.set_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw new_yaw = CarlaDataProvider.get_map().get_waypoint( next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: angular_velocity.z = 0 else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) self._actor.set_angular_velocity(angular_velocity) self._last_update = current_time return direction_norm
def gather_info(self, ego_control_and_speed_info=None): # if self.step % 1 == 0: # self.record_other_actor_info_for_causal_analysis(ego_control_and_speed_info) ego_bbox = get_bbox(self._vehicle) ego_front_bbox = ego_bbox[:2] actors = self._world.get_actors() vehicle_list = actors.filter('*vehicle*') pedestrian_list = actors.filter('*walker*') min_d = 10000 d_angle_norm = 1 for i, vehicle in enumerate(vehicle_list): if vehicle.id == self._vehicle.id: continue d_angle_norm_i = angle_from_center_view_fov(vehicle, self._vehicle, fov=90) d_angle_norm = np.min([d_angle_norm, d_angle_norm_i]) if d_angle_norm_i == 0: other_bbox = get_bbox(vehicle) for other_b in other_bbox: for ego_b in ego_bbox: d = norm_2d(other_b, ego_b) # print('vehicle', i, 'd', d) min_d = np.min([min_d, d]) for i, pedestrian in enumerate(pedestrian_list): d_angle_norm_i = angle_from_center_view_fov(pedestrian, self._vehicle, fov=90) d_angle_norm = np.min([d_angle_norm, d_angle_norm_i]) if d_angle_norm_i == 0: pedestrian_location = pedestrian.get_transform().location for ego_b in ego_front_bbox: d = norm_2d(pedestrian_location, ego_b) # print('pedestrian', i, 'd', d) min_d = np.min([min_d, d]) if min_d < self.min_d: self.min_d = min_d with open(self.deviations_path, 'a') as f_out: f_out.write('min_d,' + str(self.min_d) + '\n') if d_angle_norm < self.d_angle_norm: self.d_angle_norm = d_angle_norm with open(self.deviations_path, 'a') as f_out: f_out.write('d_angle_norm,' + str(self.d_angle_norm) + '\n') angle_th = 120 current_location = CarlaDataProvider.get_location(self._vehicle) current_transform = CarlaDataProvider.get_transform(self._vehicle) current_waypoint = self._map.get_waypoint(current_location, project_to_road=False, lane_type=carla.LaneType.Any) ego_forward = current_transform.get_forward_vector() ego_forward = np.array([ego_forward.x, ego_forward.y]) ego_forward /= np.linalg.norm(ego_forward) ego_right = current_transform.get_right_vector() ego_right = np.array([ego_right.x, ego_right.y]) ego_right /= np.linalg.norm(ego_right) lane_center_waypoint = self._map.get_waypoint( current_location, lane_type=carla.LaneType.Any) lane_center_transform = lane_center_waypoint.transform lane_center_location = lane_center_transform.location lane_forward = lane_center_transform.get_forward_vector() lane_forward = np.array([lane_forward.x, lane_forward.y]) lane_forward /= np.linalg.norm(lane_forward) lane_right = current_transform.get_right_vector() lane_right = np.array([lane_right.x, lane_right.y]) lane_right /= np.linalg.norm(lane_right) dev_dist = current_location.distance(lane_center_location) # normalized to [0, 1]. 0 - same direction, 1 - opposite direction # print('ego_forward, lane_forward, np.dot(ego_forward, lane_forward)', ego_forward, lane_forward, np.dot(ego_forward, lane_forward)) dev_angle = math.acos(np.clip(np.dot(ego_forward, lane_forward), -1, 1)) / np.pi # smoothing and integrate dev_dist *= (dev_angle + 0.5) if dev_dist > self.dev_dist: self.dev_dist = dev_dist with open(self.deviations_path, 'a') as f_out: f_out.write('dev_dist,' + str(self.dev_dist) + '\n') # print(current_location, current_waypoint.lane_type, current_waypoint.is_junction) # print(lane_center_location, lane_center_waypoint.lane_type, lane_center_waypoint.is_junction) def get_d(coeff, dir, dir_label): n = 1 while n * coeff < 7: new_loc = carla.Location( current_location.x + n * coeff * dir[0], current_location.y + n * coeff * dir[1], 0) # print(coeff, dir, dir_label) # print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc) new_wp = self._map.get_waypoint(new_loc, project_to_road=False, lane_type=carla.LaneType.Any) if not (new_wp and new_wp.lane_type in [ carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional ] and np.abs(new_wp.transform.rotation.yaw % 360 - lane_center_waypoint.transform.rotation.yaw % 360) < angle_th): # if new_wp and new_wp.lane_type in [carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional]: # print('new_wp.transform.rotation.yaw, lane_center_waypoint.transform.rotation.yaw', new_wp.transform.rotation.yaw, lane_center_waypoint.transform.rotation.yaw) break else: n += 1 # if new_wp: # print(n, new_wp.transform.rotation.yaw) d = new_loc.distance(current_location) # print(d, new_loc, current_location) with open(self.deviations_path, 'a') as f_out: if new_wp and new_wp.lane_type in [ carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional ]: # print(dir_label, 'wronglane_d', d) if d < self.wronglane_d: self.wronglane_d = d f_out.write('wronglane_d,' + str(self.wronglane_d) + '\n') # print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc, 'wronglane_d,'+str(self.wronglane_d)+'\n') else: # if not new_wp: # s = 'None wp' # else: # s = new_wp.lane_type # print(dir_label, 'offroad_d', d, s, coeff) # if new_wp: # print(dir_label, 'lanetype', new_wp.lane_type) if d < self.offroad_d: self.offroad_d = d f_out.write('offroad_d,' + str(self.offroad_d) + '\n') # print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc, 'offroad_d,'+str(self.offroad_d)+'\n') if current_waypoint and not current_waypoint.is_junction: get_d(-0.1, lane_right, 'left') get_d(0.1, lane_right, 'right') get_d(-0.1, ego_right, 'ego_left') get_d(0.1, ego_right, 'ego_right') get_d(0.1, ego_forward, 'ego_forward')
def gather_info(self): def norm_2d(loc_1, loc_2): return np.sqrt((loc_1.x - loc_2.x)**2 + (loc_1.y - loc_2.y)**2) def get_bbox(vehicle): current_tra = vehicle.get_transform() current_loc = current_tra.location heading_vec = current_tra.get_forward_vector() heading_vec.z = 0 heading_vec = heading_vec / math.sqrt( math.pow(heading_vec.x, 2) + math.pow(heading_vec.y, 2)) perpendicular_vec = carla.Vector3D(-heading_vec.y, heading_vec.x, 0) extent = vehicle.bounding_box.extent x_boundary_vector = heading_vec * extent.x y_boundary_vector = perpendicular_vec * extent.y bbox = [ current_loc + carla.Location(x_boundary_vector - y_boundary_vector), current_loc + carla.Location(x_boundary_vector + y_boundary_vector), current_loc + carla.Location(-1 * x_boundary_vector - y_boundary_vector), current_loc + carla.Location(-1 * x_boundary_vector + y_boundary_vector) ] return bbox ego_bbox = get_bbox(self._vehicle) ego_front_bbox = ego_bbox[:2] actors = self._world.get_actors() vehicle_list = actors.filter('*vehicle*') pedestrian_list = actors.filter('*walker*') min_d = 10000 for i, vehicle in enumerate(vehicle_list): if vehicle.id == self._vehicle.id: continue other_bbox = get_bbox(vehicle) for other_b in other_bbox: for ego_b in ego_bbox: d = norm_2d(other_b, ego_b) # print('vehicle', i, 'd', d) min_d = np.min([min_d, d]) for i, pedestrian in enumerate(pedestrian_list): pedestrian_location = pedestrian.get_transform().location for ego_b in ego_front_bbox: d = norm_2d(pedestrian_location, ego_b) # print('pedestrian', i, 'd', d) min_d = np.min([min_d, d]) if min_d < self.min_d: self.min_d = min_d with open(self.deviations_path, 'a') as f_out: f_out.write('min_d,' + str(self.min_d) + '\n') angle_th = 120 current_location = CarlaDataProvider.get_location(self._vehicle) current_transform = CarlaDataProvider.get_transform(self._vehicle) current_waypoint = self._map.get_waypoint(current_location, project_to_road=False, lane_type=carla.LaneType.Any) lane_center_waypoint = self._map.get_waypoint( current_location, lane_type=carla.LaneType.Any) lane_center_transform = lane_center_waypoint.transform lane_center_location = lane_center_transform.location dev_dist = current_location.distance(lane_center_location) if dev_dist > self.dev_dist: self.dev_dist = dev_dist with open(self.deviations_path, 'a') as f_out: f_out.write('dev_dist,' + str(self.dev_dist) + '\n') # print(current_location, current_waypoint.lane_type, current_waypoint.is_junction) # print(lane_center_location, lane_center_waypoint.lane_type, lane_center_waypoint.is_junction) if current_waypoint and not current_waypoint.is_junction: ego_forward = current_transform.get_forward_vector() lane_forward = lane_center_transform.get_forward_vector() dev_angle = 2 * get_angle(lane_forward.x, lane_forward.y, ego_forward.x, ego_forward.y) / np.pi # print(lane_forward, ego_forward, dev_angle) if dev_angle > 1: dev_angle = 2 - dev_angle elif dev_angle < -1: dev_angle = (-1) * (2 + dev_angle) # carla map has opposite y axis dev_angle *= -1 def get_d(coeff, dev_angle): if coeff < 0: dev_angle = 1 - dev_angle elif coeff > 0: dev_angle = dev_angle + 1 # print(dev_angle, coeff) n = 1 rv = lane_center_waypoint.transform.get_right_vector() new_loc = carla.Location( lane_center_location.x + n * coeff * rv.x, lane_center_location.y + n * coeff * rv.y, 0) new_wp = self._map.get_waypoint(new_loc, project_to_road=False, lane_type=carla.LaneType.Any) while new_wp and new_wp.lane_type in [ carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional ] and np.abs(new_wp.transform.rotation.yaw - lane_center_waypoint.transform.rotation.yaw ) < angle_th: prev_loc = new_loc n += 1 new_loc = carla.Location( lane_center_location.x + n * coeff * rv.x, lane_center_location.y + n * coeff * rv.y, 0) new_wp = self._map.get_waypoint( new_loc, project_to_road=False, lane_type=carla.LaneType.Any) # if new_wp: # print(n, new_wp.transform.rotation.yaw) d = new_loc.distance(current_location) d *= dev_angle # print(d, new_loc, current_location) with open(self.deviations_path, 'a') as f_out: if (not new_wp) or (new_wp.lane_type not in [ carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional ]): if not new_wp: s = 'None wp' else: s = new_wp.lane_type # print('offroad_d', d, s, coeff) # if new_wp: # print('lanetype', new_wp.lane_type) if d < self.offroad_d: self.offroad_d = d with open(self.deviations_path, 'a') as f_out: f_out.write('offroad_d,' + str(self.offroad_d) + '\n') else: with open(self.deviations_path, 'a') as f_out: # print('wronglane_d', d, coeff) if d < self.wronglane_d: self.wronglane_d = d f_out.write('wronglane_d,' + str(self.wronglane_d) + '\n') get_d(-0.2, dev_angle) get_d(0.2, dev_angle)
def update(self): """ Compute next control step for the given waypoint plan, obtain and apply control to actor """ new_status = py_trees.common.Status.RUNNING check_term = operator.attrgetter("terminate_WF_actor_{}".format(self._actor.id)) terminate_wf = check_term(py_trees.blackboard.Blackboard()) check_run = operator.attrgetter("running_WF_actor_{}".format(self._actor.id)) active_wf = check_run(py_trees.blackboard.Blackboard()) # Termination of WF if the WFs unique_id is listed in terminate_wf # only one WF should be active, therefore all previous WF have to be terminated if self._unique_id in terminate_wf: terminate_wf.remove(self._unique_id) if self._unique_id in active_wf: active_wf.remove(self._unique_id) py_trees.blackboard.Blackboard().set( "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True) py_trees.blackboard.Blackboard().set( "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True) new_status = py_trees.common.Status.SUCCESS return new_status if self._blackboard_queue_name is not None: while not self._queue.empty(): actor = self._queue.get() if actor is not None and actor not in self._actor_dict: self._apply_local_planner(actor) success = True for actor in self._local_planner_dict: local_planner = self._local_planner_dict[actor] if actor else None if actor is not None and actor.is_alive and local_planner is not None: # Check if the actor is a vehicle/bike if not isinstance(actor, carla.Walker): control = local_planner.run_step(debug=False) if self._avoid_collision and detect_lane_obstacle(actor): control.throttle = 0.0 control.brake = 1.0 actor.apply_control(control) # Check if the actor reached the end of the plan # @TODO replace access to private _waypoints_queue with public getter if local_planner._waypoints_queue: # pylint: disable=protected-access success = False # If the actor is a pedestrian, we have to use the WalkerAIController # The walker is sent to the next waypoint in its plan else: actor_location = CarlaDataProvider.get_location(actor) success = False if self._actor_dict[actor]: location = self._actor_dict[actor][0] direction = location - actor_location direction_norm = math.sqrt(direction.x**2 + direction.y**2) control = actor.get_control() control.speed = self._target_speed control.direction = direction / direction_norm actor.apply_control(control) if direction_norm < 1.0: self._actor_dict[actor] = self._actor_dict[actor][1:] if self._actor_dict[actor] is None: success = True else: control = actor.get_control() control.speed = self._target_speed control.direction = CarlaDataProvider.get_transform(actor).rotation.get_forward_vector() actor.apply_control(control) if success: new_status = py_trees.common.Status.SUCCESS return new_status