Python carla.Vector3D() Examples

def twist_command_updated(self, twist):
        """
        Set angular/linear velocity (this does not respect vehicle dynamics)
        """
        if not self.vehicle_control_override:
            angular_velocity = Vector3D()
            angular_velocity.z = math.degrees(twist.angular.z)

            rotation_matrix = transforms.carla_rotation_to_numpy_rotation_matrix(
                self.carla_actor.get_transform().rotation)
            linear_vector = numpy.array([twist.linear.x, twist.linear.y, twist.linear.z])
            rotated_linear_vector = rotation_matrix.dot(linear_vector)
            linear_velocity = Vector3D()
            linear_velocity.x = rotated_linear_vector[0]
            linear_velocity.y = -rotated_linear_vector[1]
            linear_velocity.z = rotated_linear_vector[2]

            rospy.logdebug("Set velocity linear: {}, angular: {}".format(
                linear_velocity, angular_velocity))
            self.carla_actor.set_velocity(linear_velocity)
            self.carla_actor.set_angular_velocity(angular_velocity) 

def __init__(self, location=None, rotation=None, matrix=None):
        if matrix is not None:
            self.matrix = matrix
            self.location = Location(matrix[0, 3], matrix[1, 3], matrix[2, 3])

            # Forward vector is retrieved from the matrix.
            self.forward_vector = Vector3D(self.matrix[0, 0],
                                           self.matrix[1, 0], self.matrix[2,
                                                                          0])
            pitch_r = math.asin(self.forward_vector.z)
            yaw_r = math.acos(
                np.clip(self.forward_vector.x / math.cos(pitch_r), -1, 1))
            roll_r = math.asin(matrix[2, 1] / (-1 * math.cos(pitch_r)))
            self.rotation = Rotation(math.degrees(pitch_r),
                                     math.degrees(yaw_r), math.degrees(roll_r))
        else:
            self.location, self.rotation = location, rotation
            self.matrix = Transform._create_matrix(self.location,
                                                   self.rotation)

            # Forward vector is retrieved from the matrix.
            self.forward_vector = Vector3D(self.matrix[0, 0],
                                           self.matrix[1, 0], self.matrix[2,
                                                                          0]) 

def draw_radar_measurement(debug_helper: carla.DebugHelper, data: carla.RadarMeasurement, velocity_range=7.5,
                           size=0.075, life_time=0.06):
    """Code adapted from carla/PythonAPI/examples/manual_control.py:
        - White: means static points.
        - Red: indicates points moving towards the object.
        - Blue: denoted points moving away.
    """
    radar_rotation = data.transform.rotation
    for detection in data:
        azimuth = math.degrees(detection.azimuth) + radar_rotation.yaw
        altitude = math.degrees(detection.altitude) + radar_rotation.pitch

        # move to local coordinates:
        forward_vec = carla.Vector3D(x=detection.depth - 0.25)
        global_to_local(forward_vec,
                        reference=carla.Rotation(pitch=altitude, yaw=azimuth, roll=radar_rotation.roll))

        # draw:
        debug_helper.draw_point(data.transform.location + forward_vec, size=size, life_time=life_time,
                                persistent_lines=False, color=carla.Color(255, 255, 255))


# -------------------------------------------------------------------------------------------------
# -- Math
# ------------------------------------------------------------------------------------------------- 

def rotate(self, angle):
        """Rotate the vector by a given angle.

        Args:
            angle (float): The angle to rotate the Vector by (in degrees).

        Returns:
            :py:class:`.Vector3D`: An instance with the coordinates of the
            rotated vector.
        """
        x_ = math.cos(math.radians(angle)) * self.x - math.sin(
            math.radians(angle)) * self.y
        y_ = math.sin(math.radians(angle)) * self.x - math.cos(
            math.radians(angle)) * self.y
        return type(self)(x_, y_, self.z) 

def _reset_world(self, soft=False):
        # init actor
        if not soft:
            spawn_point = env_utils.random_spawn_point(self.map)
        else:
            spawn_point = self.spawn_point

        if self.vehicle is None:
            blueprint = env_utils.random_blueprint(self.world, actor_filter=self.vehicle_filter)
            self.vehicle = env_utils.spawn_actor(self.world, blueprint, spawn_point)  # type: carla.Vehicle

            self._create_sensors()
            self.synchronous_context = CARLASyncContext(self.world, self.sensors, fps=self.fps)
        else:
            self.vehicle.apply_control(carla.VehicleControl())
            self.vehicle.set_velocity(carla.Vector3D(x=0.0, y=0.0, z=0.0))
            self.vehicle.set_transform(spawn_point)

        # reset reward variables
        self.collision_penalty = 0.0 

def get_trafficlight_trigger_location(traffic_light):    # pylint: disable=invalid-name
        """
        Calculates the yaw of the waypoint that represents the trigger volume of the traffic light
        """
        def rotate_point(point, angle):
            """
            rotate a given point by a given angle
            """
            x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y
            y_ = math.sin(math.radians(angle)) * point.x - math.cos(math.radians(angle)) * point.y

            return carla.Vector3D(x_, y_, point.z)

        base_transform = traffic_light.get_transform()
        base_rot = base_transform.rotation.yaw
        area_loc = base_transform.transform(traffic_light.trigger_volume.location)
        area_ext = traffic_light.trigger_volume.extent

        point = rotate_point(carla.Vector3D(0, 0, area_ext.z), base_rot)
        point_location = area_loc + carla.Location(x=point.x, y=point.y)

        return carla.Location(point_location.x, point_location.y, point_location.z) 

def from_angular_velocity(cls, angular_velocity, dt):
        """Creates a Quaternion from an angular velocity vector and the time
        delta to apply it for.

        Args:
            angular_velocity (:py:class:`.Vector3D`): The vector representing
                the angular velocity of the object in the body-frame.
            dt (float): The time delta to apply the angular velocity for.

        Returns:
            :py:class:`.Quaternion`: The quaternion representing the rotation
                undergone by the object with the given angular velocity in the
                given delta time.
        """
        angular_velocity_np = angular_velocity.as_numpy_array() * dt
        magnitude = np.linalg.norm(angular_velocity_np)

        w = np.cos(magnitude / 2.0)
        if magnitude < 1e-50:
            # To avoid instabilities and nan.
            x, y, z = 0, 0, 0
        else:
            imaginary = angular_velocity_np / magnitude * np.sin(
                magnitude / 2.0)
            x, y, z = imaginary
        return cls(w, x, y, z) 

def cross(carla_vector3d_1, carla_vector3d_2):
  """Cross product."""
  return carla.Vector3D(
      x=carla_vector3d_1.y * carla_vector3d_2.z -
      carla_vector3d_1.z * carla_vector3d_2.y,
      y=carla_vector3d_1.z * carla_vector3d_2.x -
      carla_vector3d_1.x * carla_vector3d_2.z,
      z=carla_vector3d_1.x * carla_vector3d_2.y -
      carla_vector3d_1.y * carla_vector3d_2.x) 

def vector(v):
    """ Turn carla Location/Vector3D/Rotation to np.array """
    if isinstance(v, carla.Location) or isinstance(v, carla.Vector3D):
        return np.array([v.x, v.y, v.z])
    elif isinstance(v, carla.Rotation):
        return np.array([v.pitch, v.yaw, v.roll]) 

def get_vector_length_squared(carla_vector):
        """
        Calculate the squared length of a carla_vector
        :param carla_vector: the carla vector
        :type carla_vector: carla.Vector3D
        :return: squared vector length
        :rtype: float64
        """
        return carla_vector.x * carla_vector.x + \
            carla_vector.y * carla_vector.y + \
            carla_vector.z * carla_vector.z 

def process_segmentation_images(msg,
                                camera_setup,
                                ego_vehicle,
                                speed,
                                csv,
                                dump=False):
    print("Received a message for the time: {}".format(msg.timestamp))

    # If we are in distance to the destination, stop and exit with success.
    if ego_vehicle.get_location().distance(VEHICLE_DESTINATION) <= 5:
        ego_vehicle.set_velocity(carla.Vector3D())
        CLEANUP_FUNCTION()
        sys.exit(0)

    # Compute the segmentation mIOU.
    frame = SegmentedFrame.from_carla_image(msg, camera_setup)
    compute_and_log_miou(frame, msg.timestamp, csv)

    # Visualize the run.
    if dump:
        frame.save(int(msg.timestamp * 1000), './_out/', 'seg')

    # Move the ego_vehicle according to the given speed.
    ego_vehicle.set_velocity(carla.Vector3D(x=-speed))

    # Move the simulator forward.
    ego_vehicle.get_world().tick() 

def from_carla_location(cls, location):
        """Creates a pylot Location from a CARLA location.

        Args:
            location (carla.Location): An instance of a CARLA location.

        Returns:
            :py:class:`.Location`: A pylot location.
        """
        import carla
        if not (isinstance(location, carla.Location)
                or isinstance(location, carla.Vector3D)):
            raise ValueError(
                'The location must be a carla.Location or carla.Vector3D')
        return cls(location.x, location.y, location.z) 

def __str__(self):
        return 'Vector3D(x={}, y={}, z={})'.format(self.x, self.y, self.z) 

def l2_distance(self, other):
        """Calculates the L2 distance between the point and another point.

        Args:
            other (:py:class:`~.Vector3D`): The other vector used to
                calculate the L2 distance to.

        Returns:
            :obj:`float`: The L2 distance between the two points.
        """
        vec = np.array([self.x - other.x, self.y - other.y, self.z - other.z])
        return np.linalg.norm(vec) 

def l1_distance(self, other):
        """Calculates the L1 distance between the point and another point.

        Args:
            other (:py:class:`~.Vector3D`): The other vector used to
                calculate the L1 distance to.

        Returns:
            :obj:`float`: The L1 distance between the two points.
        """
        return abs(self.x - other.x) + abs(self.y - other.y) + abs(self.z -
                                                                   other.z) 

def as_carla_vector(self):
        """Retrieves the 3D vector as an instance of CARLA 3D vector.

        Returns:
            carla.Vector3D: Instance representing the 3D vector.
        """
        import carla
        return carla.Vector3D(self.x, self.y, self.z) 

def from_carla_vector(cls, vector):
        """Creates a pylot Vector3D from a CARLA 3D vector.

        Args:
            vector (carla.Vector3D): An instance of a CARLA 3D vector.

        Returns:
            :py:class:`.Vector3D`: A pylot 3D vector.
        """
        import carla
        if not isinstance(vector, carla.Vector3D):
            raise ValueError('The vector must be a carla.Vector3D')
        return cls(vector.x, vector.y, vector.z) 

def cosine_similarity(a: carla.Vector3D, b: carla.Vector3D) -> float:
    """-1: opposite vectors (pointing in the opposite direction),
        0: orthogonal,
        1: exactly the same (pointing in the same direction)
    """
    return dot_product(a, b) / (vector_norm(a) * vector_norm(b)) 

def dot_product(a: carla.Vector3D, b: carla.Vector3D) -> float:
    return a.x * b.x + a.y * b.y + a.z * b.z 

def vector_norm(vec: carla.Vector3D) -> float:
    """Returns the norm/magnitude (a scalar) of the given 3D vector."""
    return math.sqrt(vec.x**2 + vec.y**2 + vec.z**2) 

def use_sample(self, sample):
        print('Sample:', sample)

        init_conds = sample.init_conditions
        self.target_speed = init_conds.target_speed[0]

        # Ego vehicle physics parameters
        com = carla.Vector3D(
            init_conds.center_of_mass[0],
            init_conds.center_of_mass[1],
            init_conds.center_of_mass[2],
        )
        wheels = [
            carla.WheelPhysicsControl(tire_friction=init_conds.tire_friction[0])
            for _ in range(4)
        ]
        self.vehicle_mass = init_conds.mass[0]
        physics = carla.VehiclePhysicsControl(
            mass=self.vehicle_mass,
            max_rpm=init_conds.max_rpm[0],
            center_of_mass=com,
            drag_coefficient=init_conds.drag_coefficient[0],
            wheels=wheels
        )

        # PID controller parameters
        opt_dict = {
            'target_speed': self.target_speed
        }

        # Deterministic blueprint, spawnpoint.
        blueprint = self.world.world.get_blueprint_library().find('vehicle.tesla.model3')
        spawn = self.world.map.get_spawn_points()[0]

        self.world.add_vehicle(PIDAgent, control_params=opt_dict,
                               blueprint=blueprint, spawn=spawn,
                               physics=physics, has_collision_sensor=True,
                               has_lane_sensor=True, ego=True) 

def initialise(self):
        if self._actor.is_alive:
            self._actor.set_velocity(carla.Vector3D(0, 0, 0))
            self._actor.set_angular_velocity(carla.Vector3D(0, 0, 0))
            self._actor.set_transform(self._transform)
        super(ActorTransformSetter, self).initialise() 

def initialise(self):
        """
        Initialize it's speed
        """

        transform = self._actor.get_transform()
        yaw = transform.rotation.yaw * (math.pi / 180)

        vx = math.cos(yaw) * self._init_speed
        vy = math.sin(yaw) * self._init_speed
        self._actor.set_velocity(carla.Vector3D(vx, vy, 0)) 

def initialise(self):

        super(ActorTransformSetterToOSCPosition, self).initialise()

        if self._actor.is_alive:
            self._actor.set_velocity(carla.Vector3D(0, 0, 0))
            self._actor.set_angular_velocity(carla.Vector3D(0, 0, 0)) 

def rotate_point(self, point, angle):
        """
        rotate a given point by a given angle
        """
        x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y
        y_ = math.sin(math.radians(angle)) * point.x - math.cos(math.radians(angle)) * point.y
        return carla.Vector3D(x_, y_, point.z) 

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 run_step(self):
        """
        Execute on tick of the controller's control loop

        If _waypoints are provided, the vehicle moves towards the next waypoint
        with the given _target_speed, until reaching the final waypoint. Upon reaching
        the final waypoint, _reached_goal is set to True.

        If _waypoints is empty, the vehicle moves in its current direction with
        the given _target_speed.

        If _init_speed is True, the control command is post-processed to ensure that
        the initial actor velocity is maintained independent of physics.
        """
        self._reached_goal = False
        self._local_planner.set_speed(self._target_speed * 3.6)

        if self._waypoints_updated:
            self._waypoints_updated = False
            self._update_plan()

        control = self._local_planner.run_step(debug=False)

        # Check if the actor reached the end of the plan
        # @TODO replace access to private _waypoints_queue with public getter
        if not self._local_planner._waypoints_queue:  # pylint: disable=protected-access
            self._reached_goal = True

        self._actor.apply_control(control)

        if self._init_speed:
            current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2)

            if abs(self._target_speed - current_speed) > 3:
                yaw = self._actor.get_transform().rotation.yaw * (math.pi / 180)
                vx = math.cos(yaw) * self._target_speed
                vy = math.sin(yaw) * self._target_speed
                self._actor.set_velocity(carla.Vector3D(vx, vy, 0)) 

def run_step(self):
        """
        Execute on tick of the controller's control loop

        The control loop is very simplistic:
            If the actor speed is below the _target_speed, set throttle to 1.0,
            otherwise, set throttle to 0.0
        Note, that this is a longitudinal controller only.

        If _init_speed is True, the control command is post-processed to ensure that
        the initial actor velocity is maintained independent of physics.
        """

        control = self._actor.get_control()

        velocity = self._actor.get_velocity()
        current_speed = math.sqrt(velocity.x**2 + velocity.y**2)
        if current_speed < self._target_speed:
            control.throttle = 1.0
        else:
            control.throttle = 0.0

        self._actor.apply_control(control)

        if self._init_speed:
            if abs(self._target_speed - current_speed) > 3:
                yaw = self._actor.get_transform().rotation.yaw * (math.pi / 180)
                vx = math.cos(yaw) * self._target_speed
                vy = math.sin(yaw) * self._target_speed
                self._actor.set_velocity(carla.Vector3D(vx, vy, 0))