Check if a given vehicle is an obstacle in our way. To this end we take into account the road and lane the target vehicle is on and run a geometry test to check if the target vehicle is under a certain distance in front of our ego vehicle. We also check the next wayp
(self, ego_wpt, ego_loc, vehicle_list,
proximity_th, up_angle_th, low_angle_th=0, lane_offset=0)
| 131 | return carla.Location(point_location.x, point_location.y, point_location.z) |
| 132 | |
| 133 | def _bh_is_vehicle_hazard(self, ego_wpt, ego_loc, vehicle_list, |
| 134 | proximity_th, up_angle_th, low_angle_th=0, lane_offset=0): |
| 135 | """ |
| 136 | Check if a given vehicle is an obstacle in our way. To this end we take |
| 137 | into account the road and lane the target vehicle is on and run a |
| 138 | geometry test to check if the target vehicle is under a certain distance |
| 139 | in front of our ego vehicle. We also check the next waypoint, just to be |
| 140 | sure there's not a sudden road id change. |
| 141 | |
| 142 | WARNING: This method is an approximation that could fail for very large |
| 143 | vehicles, which center is actually on a different lane but their |
| 144 | extension falls within the ego vehicle lane. Also, make sure to remove |
| 145 | the ego vehicle from the list. Lane offset is set to +1 for right lanes |
| 146 | and -1 for left lanes, but this has to be inverted if lane values are |
| 147 | negative. |
| 148 | |
| 149 | :param ego_wpt: waypoint of ego-vehicle |
| 150 | :param ego_log: location of ego-vehicle |
| 151 | :param vehicle_list: list of potential obstacle to check |
| 152 | :param proximity_th: threshold for the agent to be alerted of |
| 153 | a possible collision |
| 154 | :param up_angle_th: upper threshold for angle |
| 155 | :param low_angle_th: lower threshold for angle |
| 156 | :param lane_offset: for right and left lane changes |
| 157 | :return: a tuple given by (bool_flag, vehicle, distance), where: |
| 158 | - bool_flag is True if there is a vehicle ahead blocking us |
| 159 | and False otherwise |
| 160 | - vehicle is the blocker object itself |
| 161 | - distance is the meters separating the two vehicles |
| 162 | """ |
| 163 | |
| 164 | # Get the right offset |
| 165 | if ego_wpt.lane_id < 0 and lane_offset != 0: |
| 166 | lane_offset *= -1 |
| 167 | |
| 168 | for target_vehicle in vehicle_list: |
| 169 | |
| 170 | target_vehicle_loc = target_vehicle.get_location() |
| 171 | # If the object is not in our next or current lane it's not an obstacle |
| 172 | |
| 173 | target_wpt = self._map.get_waypoint(target_vehicle_loc) |
| 174 | if target_wpt.road_id != ego_wpt.road_id or \ |
| 175 | target_wpt.lane_id != ego_wpt.lane_id + lane_offset: |
| 176 | next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0] |
| 177 | if target_wpt.road_id != next_wpt.road_id or \ |
| 178 | target_wpt.lane_id != next_wpt.lane_id + lane_offset: |
| 179 | continue |
| 180 | |
| 181 | if is_within_distance(target_vehicle_loc, ego_loc, |
| 182 | self._vehicle.get_transform().rotation.yaw, |
| 183 | proximity_th, up_angle_th, low_angle_th): |
| 184 | |
| 185 | return (True, target_vehicle, compute_distance(target_vehicle_loc, ego_loc)) |
| 186 | |
| 187 | return (False, None, -1) |
| 188 | |
| 189 | def _is_vehicle_hazard(self, vehicle_list): |
| 190 | """ |
no test coverage detected