#!/usr/bin/env python

# Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
""" This module provides a helper for the co-simulation between vissim and carla. """

# ==================================================================================================
# -- imports ---------------------------------------------------------------------------------------
# ==================================================================================================

import logging
import math
import random

import carla  # pylint: disable=import-error

# ==================================================================================================
# -- Bridge helper (VISSIM <=> CARLA) --------------------------------------------------------------
# ==================================================================================================


class BridgeHelper(object):
    """
    BridgeHelper provides methos to ease the co-simulation between vissim and carla.
    """

    blueprint_library = []
    vtypes = {}

    @staticmethod
    def get_carla_transform(in_vissim_transform, extent=None):
        """
        Returns carla transform based on vissim transform.
        """
        in_location = in_vissim_transform.location
        in_rotation = in_vissim_transform.rotation

        # From front-center-bumper to center (vissim reference system).
        if extent is not None:
            out_location = (in_location.x - math.cos(math.radians(in_rotation.yaw)) * extent.x,
                            in_location.y - math.sin(math.radians(in_rotation.yaw)) * extent.x,
                            in_location.z - math.sin(math.radians(in_rotation.pitch)) * extent.x)
        else:
            out_location = (in_location.x, in_location.y, in_location.z)
        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)

        # Transform to carla reference system (left-handed system).
        out_transform = carla.Transform(
            carla.Location(out_location[0], -out_location[1], out_location[2]),
            carla.Rotation(out_rotation[0], -out_rotation[1], out_rotation[2]))

        return out_transform

    @staticmethod
    def get_vissim_transform(in_carla_transform, extent=None):
        """
        Returns vissim transform based on carla transform.
        """
        in_location = in_carla_transform.location
        in_rotation = in_carla_transform.rotation

        # From center to front-center-bumper (carla reference system).
        if extent is not None:
            yaw = -1 * in_rotation.yaw
            pitch = in_rotation.pitch
            out_location = (in_location.x + math.cos(math.radians(yaw)) * extent.x,
                            in_location.y - math.sin(math.radians(yaw)) * extent.x,
                            in_location.z - math.sin(math.radians(pitch)) * extent.x)
        else:
            out_location = (in_location.x, in_location.y, in_location.z)
        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)

        # Transform to vissim reference system (right-handed system).
        out_transform = carla.Transform(
            carla.Location(out_location[0], -out_location[1], out_location[2]),
            carla.Rotation(out_rotation[0], -out_rotation[1], out_rotation[2]))

        return out_transform

    @staticmethod
    def _flip_y(in_vector):
        """
        Flips y coordinate of the given vector.
        """
        return carla.Vector3D(in_vector.x, -in_vector.y, in_vector.z)

    @staticmethod
    def get_carla_velocity(in_vissim_velocity):
        """
        Returns carla velocity based on vissim velocity.
        """
        return BridgeHelper._flip_y(in_vissim_velocity)

    @staticmethod
    def get_vissim_velocity(in_carla_velocity):
        """
        Returns vissim velocity based on carla velocity.
        """
        return BridgeHelper._flip_y(in_carla_velocity)

    @staticmethod
    def _get_recommended_carla_blueprint(vissim_actor):
        """
        Returns an appropriate blueprint based on the given vissim actor.
        """
        blueprint = BridgeHelper.blueprint_library.filter('vehicle.seat.leon')[0]
        color = random.choice(blueprint.get_attribute('color').recommended_values)
        blueprint.set_attribute('color', color)
        return blueprint

    @staticmethod
    def get_carla_blueprint(vissim_actor):
        """
        Returns an appropriate blueprint based on the received vissim actor.
        """
        type_id = str(vissim_actor.type)

        if type_id in BridgeHelper.vtypes:
            candidates = BridgeHelper.vtypes[type_id]
            if candidates:
                blueprint_id = random.choice(candidates)
            else:
                logging.error(
                    'vissim type %s not supported. No vehicle will be spawned in carla', type_id)
                return None

            blueprint = BridgeHelper.blueprint_library.filter(blueprint_id)
            if not blueprint:
                logging.error(
                    'carla blueprint %s unknown. No vehicle will be spawned', blueprint_id)
                return None
            blueprint = blueprint[0]

            if blueprint.has_attribute('color'):
                color = random.choice(blueprint.get_attribute('color').recommended_values)
                blueprint.set_attribute('color', color)

            if blueprint.has_attribute('driver_id'):
                driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
                blueprint.set_attribute('driver_id', driver_id)

            blueprint.set_attribute('role_name', 'vissim_driver')
            return blueprint

        else:
            logging.error(
                'vissim type %s unknown. No vehicle will be spawned in carla', type_id)
            return None

#!/usr/bin/env python

# Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
""" This module provides a helper for the co-simulation between vissim and carla. """

# ==================================================================================================
# -- imports ---------------------------------------------------------------------------------------
# ==================================================================================================

import logging
import math
import random

import carla  # pylint: disable=import-error

# ==================================================================================================
# -- Bridge helper (VISSIM <=> CARLA) --------------------------------------------------------------
# ==================================================================================================


class BridgeHelper(object):
    """
    BridgeHelper provides methos to ease the co-simulation between vissim and carla.
    """

    blueprint_library = []
    vtypes = {}

    @staticmethod
    def get_carla_transform(in_vissim_transform, extent=None):
        """
        Returns carla transform based on vissim transform.
        """
        in_location = in_vissim_transform.location
        in_rotation = in_vissim_transform.rotation

        # From front-center-bumper to center (vissim reference system).
        if extent is not None:
            out_location = (in_location.x - math.cos(math.radians(in_rotation.yaw)) * extent.x,
                            in_location.y - math.sin(math.radians(in_rotation.yaw)) * extent.x,
                            in_location.z - math.sin(math.radians(in_rotation.pitch)) * extent.x)
        else:
            out_location = (in_location.x, in_location.y, in_location.z)
        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)

        # Transform to carla reference system (left-handed system).
        out_transform = carla.Transform(
            carla.Location(out_location[0], -out_location[1], out_location[2]),
            carla.Rotation(out_rotation[0], -out_rotation[1], out_rotation[2]))

        return out_transform

    @staticmethod
    def get_vissim_transform(in_carla_transform, extent=None):
        """
        Returns vissim transform based on carla transform.
        """
        in_location = in_carla_transform.location
        in_rotation = in_carla_transform.rotation

        # From center to front-center-bumper (carla reference system).
        if extent is not None:
            yaw = -1 * in_rotation.yaw
            pitch = in_rotation.pitch
            out_location = (in_location.x + math.cos(math.radians(yaw)) * extent.x,
                            in_location.y - math.sin(math.radians(yaw)) * extent.x,
                            in_location.z - math.sin(math.radians(pitch)) * extent.x)
        else:
            out_location = (in_location.x, in_location.y, in_location.z)
        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)

        # Transform to vissim reference system (right-handed system).
        out_transform = carla.Transform(
            carla.Location(out_location[0], -out_location[1], out_location[2]),
            carla.Rotation(out_rotation[0], -out_rotation[1], out_rotation[2]))

        return out_transform

    @staticmethod
    def _flip_y(in_vector):
        """
        Flips y coordinate of the given vector.
        """
        return carla.Vector3D(in_vector.x, -in_vector.y, in_vector.z)

    @staticmethod
    def get_carla_velocity(in_vissim_velocity):
        """
        Returns carla velocity based on vissim velocity.
        """
        return BridgeHelper._flip_y(in_vissim_velocity)

    @staticmethod
    def get_vissim_velocity(in_carla_velocity):
        """
        Returns vissim velocity based on carla velocity.
        """
        return BridgeHelper._flip_y(in_carla_velocity)

    @staticmethod
    def _get_recommended_carla_blueprint(vissim_actor):
        """
        Returns an appropriate blueprint based on the given vissim actor.
        """
        blueprint = BridgeHelper.blueprint_library.filter('vehicle.seat.leon')[0]
        color = random.choice(blueprint.get_attribute('color').recommended_values)
        blueprint.set_attribute('color', color)
        return blueprint

    @staticmethod
    def get_carla_blueprint(vissim_actor):
        """
        Returns an appropriate blueprint based on the received vissim actor.
        """
        type_id = str(vissim_actor.type)

        if type_id in BridgeHelper.vtypes:
            candidates = BridgeHelper.vtypes[type_id]
            if candidates:
                blueprint_id = random.choice(candidates)
            else:
                logging.error(
                    'vissim type %s not supported. No vehicle will be spawned in carla', type_id)
                return None

            blueprint = BridgeHelper.blueprint_library.filter(blueprint_id)
            if not blueprint:
                logging.error(
                    'carla blueprint %s unknown. No vehicle will be spawned', blueprint_id)
                return None
            blueprint = blueprint[0]

            if blueprint.has_attribute('color'):
                color = random.choice(blueprint.get_attribute('color').recommended_values)
                blueprint.set_attribute('color', color)

            if blueprint.has_attribute('driver_id'):
                driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
                blueprint.set_attribute('driver_id', driver_id)

            blueprint.set_attribute('role_name', 'vissim_driver')
            return blueprint

        else:
            logging.error(
                'vissim type %s unknown. No vehicle will be spawned in carla', type_id)
            return None

#!/usr/bin/env python # Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de # Barcelona (UAB). # # This work is licensed under the terms of the MIT license. # For a copy, see <https://opensource.org/licenses/MIT>. This module provides a helper for the co-simulation between vissim and carla. # ================================================================================================== # -- imports --------------------------------------------------------------------------------------- # ================================================================================================== # pylint: disable=import-error # ================================================================================================== # -- Bridge helper (VISSIM <=> CARLA) -------------------------------------------------------------- # ================================================================================================== BridgeHelper provides methos to ease the co-simulation between vissim and carla. Returns carla transform based on vissim transform. # From front-center-bumper to center (vissim reference system). # Transform to carla reference system (left-handed system). Returns vissim transform based on carla transform. # From center to front-center-bumper (carla reference system). # Transform to vissim reference system (right-handed system). Flips y coordinate of the given vector. Returns carla velocity based on vissim velocity. Returns vissim velocity based on carla velocity. Returns an appropriate blueprint based on the given vissim actor. Returns an appropriate blueprint based on the received vissim actor.