Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint (next_waypoint). :param current_waypoint: active waypoint :param next_waypoint: target waypoint :return: the type of topological connection encoded as a RoadOption enu
(current_waypoint, next_waypoint, threshold=35)
| 302 | |
| 303 | |
| 304 | def _compute_connection(current_waypoint, next_waypoint, threshold=35): |
| 305 | """ |
| 306 | Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint |
| 307 | (next_waypoint). |
| 308 | |
| 309 | :param current_waypoint: active waypoint |
| 310 | :param next_waypoint: target waypoint |
| 311 | :return: the type of topological connection encoded as a RoadOption enum: |
| 312 | RoadOption.STRAIGHT |
| 313 | RoadOption.LEFT |
| 314 | RoadOption.RIGHT |
| 315 | """ |
| 316 | n = next_waypoint.transform.rotation.yaw |
| 317 | n = n % 360.0 |
| 318 | |
| 319 | c = current_waypoint.transform.rotation.yaw |
| 320 | c = c % 360.0 |
| 321 | |
| 322 | diff_angle = (n - c) % 180.0 |
| 323 | if diff_angle < threshold or diff_angle > (180 - threshold): |
| 324 | return RoadOption.STRAIGHT |
| 325 | elif diff_angle > 90.0: |
| 326 | return RoadOption.LEFT |
| 327 | else: |
| 328 | return RoadOption.RIGHT |