#!/usr/bin/env python
# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
""" Module with auxiliary functions. """
import math
import carla
import numpy as np
[docs]def draw_waypoints(world, waypoints, z=0.5):
"""
Draw a list of waypoints at a certain height given in z.
:param world: carla.world object
:param waypoints: list or iterable container with the waypoints to draw
:param z: height in meters
"""
for wpt in waypoints:
wpt_t = wpt.transform
begin = wpt_t.location + carla.Location(z=z)
angle = math.radians(wpt_t.rotation.yaw)
end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle))
world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0)
[docs]def get_speed(vehicle):
"""
Compute speed of a vehicle in Km/h.
:param vehicle: the vehicle for which speed is calculated
:return: speed as a float in Km/h
"""
vel = vehicle.get_velocity()
return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
[docs]def is_within_distance_ahead(target_transform, current_transform, max_distance):
"""
Check if a target object is within a certain distance in front of a reference object.
:param target_transform: location of the target object
:param current_transform: location of the reference object
:param orientation: orientation of the reference object
:param max_distance: maximum allowed distance
:return: True if target object is within max_distance ahead of the reference object
"""
target_vector = np.array(
[
target_transform.location.x - current_transform.location.x,
target_transform.location.y - current_transform.location.y,
]
)
norm_target = np.linalg.norm(target_vector)
# If the vector is too short, we can simply stop here
if norm_target < 0.001:
return True
if norm_target > max_distance:
return False
fwd = current_transform.get_forward_vector()
forward_vector = np.array([fwd.x, fwd.y])
d_angle = math.degrees(
math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1.0, 1.0))
)
return d_angle < 90.0
[docs]def is_within_distance(
target_location,
current_location,
orientation,
max_distance,
d_angle_th_up,
d_angle_th_low=0,
):
"""
Check if a target object is within a certain distance from a reference object.
A vehicle in front would be something around 0 deg, while one behind around 180 deg.
:param target_location: location of the target object
:param current_location: location of the reference object
:param orientation: orientation of the reference object
:param max_distance: maximum allowed distance
:param d_angle_th_up: upper thereshold for angle
:param d_angle_th_low: low thereshold for angle (optional, default is 0)
:return: True if target object is within max_distance ahead of the reference object
"""
target_vector = np.array(
[target_location.x - current_location.x, target_location.y - current_location.y]
)
norm_target = np.linalg.norm(target_vector)
# If the vector is too short, we can simply stop here
if norm_target < 0.001:
return True
if norm_target > max_distance:
return False
forward_vector = np.array(
[math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]
)
d_angle = math.degrees(
math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1.0, 1.0))
)
return d_angle_th_low < d_angle < d_angle_th_up
[docs]def compute_magnitude_angle(target_location, current_location, orientation):
"""
Compute relative angle and distance between a target_location and a current_location
:param target_location: location of the target object
:param current_location: location of the reference object
:param orientation: orientation of the reference object
:return: a tuple composed by the distance to the object and the angle between both objects
"""
target_vector = np.array(
[target_location.x - current_location.x, target_location.y - current_location.y]
)
norm_target = np.linalg.norm(target_vector)
forward_vector = np.array(
[math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]
)
d_angle = math.degrees(
math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1.0, 1.0))
)
return (norm_target, d_angle)
[docs]def distance_vehicle(waypoint, vehicle_transform):
"""
Returns the 2D distance from a waypoint to a vehicle
:param waypoint: actual waypoint
:param vehicle_transform: transform of the target vehicle
"""
loc = vehicle_transform.location
x = waypoint.transform.location.x - loc.x
y = waypoint.transform.location.y - loc.y
return math.sqrt(x * x + y * y)
[docs]def vector(location_1, location_2):
"""
Returns the unit vector from location_1 to location_2
:param location_1, location_2: carla.Location objects
"""
x = location_2.x - location_1.x
y = location_2.y - location_1.y
z = location_2.z - location_1.z
norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps
return [x / norm, y / norm, z / norm]
[docs]def compute_distance(location_1, location_2):
"""
Euclidean distance between 3D points
:param location_1, location_2: 3D points
"""
x = location_2.x - location_1.x
y = location_2.y - location_1.y
z = location_2.z - location_1.z
norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps
return norm
[docs]def positive(num):
"""
Return the given number if positive, else 0
:param num: value to check
"""
return num if num > 0.0 else 0.0