fixed setup.py issue

autowarefoundation · maxime-clem · Jul 19, 2024 · Mar 15, 2024 · Mar 15, 2024 · Mar 15, 2024
commit bda3830473db7b764d21393ed18928443d5ad126
@@ -26,7 +26,7 @@
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
-            "autoware_carla_interface = autoware_carla_interface.autoware_carla_interface:main"
+            "autoware_carla_interface = autoware_carla_interface.carla_autoware:main"
         ],
     },
     package_dir={"": "src"},

diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py
        self.use_traffic_manager = self.param_["use_traffic_manager"]
        self.max_real_delta_seconds = self.param_["max_real_delta_seconds"]

    def load_world(self):
        client = carla.Client(self.local_host, self.port)
        client.set_timeout(self.timeout)
        client.load_world(self.carla_map)
        self.world = client.get_world()
        settings = self.world.get_settings()
        settings.fixed_delta_seconds = self.fixed_delta_seconds
        settings.synchronous_mode = self.sync_mode
        self.world.apply_settings(settings)
        CarlaDataProvider.set_world(self.world)
        CarlaDataProvider.set_client(client)
        spawn_point = carla.Transform()
        point_items = self.spawn_point.split(",")
        randomize = False
        if len(point_items) == 6:
            spawn_point.location.x = float(point_items[0])
            spawn_point.location.y = float(point_items[1])
            spawn_point.location.z = (
                float(point_items[2]) + 2
            )  # +2 is used so the car did not stuck on the road when spawned.
            spawn_point.rotation.roll = float(point_items[3])
            spawn_point.rotation.pitch = float(point_items[4])
            spawn_point.rotation.yaw = float(point_items[5])
        else:
            randomize = True
        self.ego_actor = CarlaDataProvider.request_new_actor(
            self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
        )
        self.interface.ego_actor = self.ego_actor  # TODO improve design
        self.interface.physics_control = self.ego_actor.get_physics_control()

        self.sensor_wrapper = SensorWrapper(self.interface)
        self.sensor_wrapper.setup_sensors(self.ego_actor, False)
        ##########################################################################################################################################################
        # TRAFFIC MANAGER
        ##########################################################################################################################################################
        # cspell:ignore trafficmanager
        if self.use_traffic_manager:
            traffic_manager = client.get_trafficmanager()
            traffic_manager.set_synchronous_mode(True)
            traffic_manager.set_random_device_seed(0)
            random.seed(0)
            spawn_points_tm = self.world.get_map().get_spawn_points()
            for i, spawn_point in enumerate(spawn_points_tm):
                self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
            models = [
                "dodge",
                "audi",
                "model3",
                "mini",
                "mustang",
                "lincoln",
                "prius",
                "nissan",
                "crown",
                "impala",
            ]
            blueprints = []
            for vehicle in self.world.get_blueprint_library().filter("*vehicle*"):
                if any(model in vehicle.id for model in models):
                    blueprints.append(vehicle)
            max_vehicles = 30
            max_vehicles = min([max_vehicles, len(spawn_points_tm)])
            vehicles = []
            for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
                temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
                if temp is not None:
                    vehicles.append(temp)

            for vehicle in vehicles:
                vehicle.set_autopilot(True)

    def run_bridge(self):
        self.bridge_loop = SensorLoop()
        self.bridge_loop.sensor = self.sensor_wrapper
        self.bridge_loop.ego_actor = self.ego_actor
        self.bridge_loop.start_system_time = time.time()
        self.bridge_loop.start_game_time = GameTime.get_time()
        self.bridge_loop.running = True
        while self.bridge_loop.running:
            timestamp = None
            world = CarlaDataProvider.get_world()
            if world:
                snapshot = world.get_snapshot()
                if snapshot:
                    timestamp = snapshot.timestamp
            if timestamp:
                delta_step = time.time() - self.prev_tick_wall_time
                if delta_step <= self.max_real_delta_seconds:
                    # Add a wait to match the max_real_delta_seconds
                    time.sleep(self.max_real_delta_seconds - delta_step)
                self.prev_tick_wall_time = time.time()
                self.bridge_loop._tick_sensor(timestamp)

    def _stop_loop(self, sign, frame):
        self.bridge_loop._stop_loop()
diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py


 class carla_ros2_interface(object):
    def __init__(self):
        self.sensor_interface = SensorInterface()
        self.timestamp = None
        self.ego_actor = None
        self.physics_control = None
        self.channels = 0
        self.id_to_sensor_type_map = {}
        self.id_to_camera_info_map = {}
        self.cv_bridge = CvBridge()
        self.first_ = True
        self.pub_lidar = {}
        self.sensor_frequencies = {
            "top": 11,
            "left": 11,
            "right": 11,
            "camera": 11,
            "imu": 50,
            "status": 50,
            "pose": 2,
        }
        self.publish_prev_times = {
            sensor: datetime.datetime.now() for sensor in self.sensor_frequencies
        }

        # initialize ros2 node
        rclpy.init(args=None)
        self.ros2_node = rclpy.create_node("carla_ros2_interface")
        self.parameters = {
            "host": rclpy.Parameter.Type.STRING,
            "port": rclpy.Parameter.Type.INTEGER,
            "sync_mode": rclpy.Parameter.Type.BOOL,
            "timeout": rclpy.Parameter.Type.INTEGER,
            "fixed_delta_seconds": rclpy.Parameter.Type.DOUBLE,
            "carla_map": rclpy.Parameter.Type.STRING,
            "ego_vehicle_role_name": rclpy.Parameter.Type.STRING,
            "spawn_point": rclpy.Parameter.Type.STRING,
            "vehicle_type": rclpy.Parameter.Type.STRING,
            "objects_definition_file": rclpy.Parameter.Type.STRING,
            "use_traffic_manager": rclpy.Parameter.Type.BOOL,
            "max_real_delta_seconds": rclpy.Parameter.Type.DOUBLE,
        }
        self.param_values = {}
        for param_name, param_type in self.parameters.items():
            self.ros2_node.declare_parameter(param_name, param_type)
            self.param_values[param_name] = self.ros2_node.get_parameter(param_name).value

        # Publish clock
        self.clock_publisher = self.ros2_node.create_publisher(Clock, "/clock", 10)
        obj_clock = Clock()
        obj_clock.clock = Time(sec=0)
        self.clock_publisher.publish(obj_clock)

        # Sensor Config (Edit your sensor here)
        self.sensors = json.load(open(self.param_values["objects_definition_file"]))

        # Subscribing Autoware Control messages and converting to CARLA control
        self.sub_control = self.ros2_node.create_subscription(
            ActuationCommandStamped, "/control/command/actuation_cmd", self.control_callback, 1
        )

        self.sub_vehicle_initialpose = self.ros2_node.create_subscription(
            PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1
        )

        self.current_control = carla.VehicleControl()

        # Direct data publishing from CARLA for Autoware
        self.pub_pose_with_cov = self.ros2_node.create_publisher(
            PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1
        )
        self.pub_vel_state = self.ros2_node.create_publisher(
            VelocityReport, "/vehicle/status/velocity_status", 1
        )
        self.pub_steering_state = self.ros2_node.create_publisher(
            SteeringReport, "/vehicle/status/steering_status", 1
        )
        self.pub_ctrl_mode = self.ros2_node.create_publisher(
            ControlModeReport, "/vehicle/status/control_mode", 1
        )
        self.pub_gear_state = self.ros2_node.create_publisher(
            GearReport, "/vehicle/status/gear_status", 1
        )
        self.pub_actuation_status = self.ros2_node.create_publisher(
            ActuationStatusStamped, "/vehicle/status/actuation_status", 1
        )

        # Create Publisher for each Physical Sensors
        for sensor in self.sensors["sensors"]:
            self.id_to_sensor_type_map[sensor["id"]] = sensor["type"]
            if sensor["type"] == "sensor.camera.rgb":
                self.pub_camera = self.ros2_node.create_publisher(
                    Image, "/sensing/camera/traffic_light/image_raw", 1
                )
                self.pub_camera_info = self.ros2_node.create_publisher(
                    CameraInfo, "/sensing/camera/traffic_light/camera_info", 1
                )
            elif sensor["type"] == "sensor.lidar.ray_cast":
                if sensor["id"] in self.sensor_frequencies:
                    self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher(
                        PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10
                    )
                else:
                    self.ros2_node.get_logger().info(
                        "Please use Top, Right, or Left as the LIDAR ID"
                    )
            elif sensor["type"] == "sensor.other.imu":
                self.pub_imu = self.ros2_node.create_publisher(
                    Imu, "/sensing/imu/tamagawa/imu_raw", 1
                )
            else:
                self.ros2_node.get_logger().info(f'No Publisher for {sensor["type"]} Sensor')
                pass

        self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,))
        self.spin_thread.start()

    def __call__(self):
        input_data = self.sensor_interface.get_data()
diff --git a/...ator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py b/...ator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py
 # Copyright 2024 Tier IV, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
        return CarlaDataProvider._local_planner

    @staticmethod
    def register_actor(actor, transform=None):
        """Add new actor to dictionaries."""
        with CarlaDataProvider._lock:
            if actor in CarlaDataProvider._actor_velocity_map:
                raise KeyError(
                    "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)
                )
            else:
                CarlaDataProvider._actor_velocity_map[actor] = 0.0
            if actor in CarlaDataProvider._actor_location_map:
                raise KeyError(
                    "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)
                )
            elif transform:
                CarlaDataProvider._actor_location_map[actor] = transform.location
            else:
                CarlaDataProvider._actor_location_map[actor] = None

            if actor in CarlaDataProvider._actor_transform_map:
                raise KeyError(
                    "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)
                )
            else:
                CarlaDataProvider._actor_transform_map[actor] = transform

    @staticmethod
    def update_osc_global_params(parameters):
            CarlaDataProvider.register_actor(actor, transform)

    @staticmethod
    def on_carla_tick():
        with CarlaDataProvider._lock:
            for actor in CarlaDataProvider._actor_velocity_map:
                if actor is not None and actor.is_alive:
                    CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity(actor)

            for actor in CarlaDataProvider._actor_location_map:
                if actor is not None and actor.is_alive:
                    CarlaDataProvider._actor_location_map[actor] = actor.get_location()

            for actor in CarlaDataProvider._actor_transform_map:
                if actor is not None and actor.is_alive:
                    CarlaDataProvider._actor_transform_map[actor] = actor.get_transform()

            world = CarlaDataProvider._world
            if world is None:
                print("WARNING: CarlaDataProvider couldn't find the world")

            CarlaDataProvider._all_actors = None

    @staticmethod
    def get_velocity(actor):
        return False

    @staticmethod
    def get_road_lanes(wp):
        if wp.is_junction:
            return []
        # find the most left lane's waypoint

        lane_id_set = set()
        pre_left = wp
        while wp and wp.lane_type == carla.LaneType.Driving:
            if wp.lane_id in lane_id_set:
                break
            lane_id_set.add(wp.lane_id)

            # carla bug: get_left_lane Return error，and never Return none. It's a infinite loop.
            pre_left = wp
            wp = wp.get_left_lane()

        # # Store data from the left lane to the right lane
        # # list<key, value>, key=laneid, value=waypoint
        lane_list = []
        lane_id_set.clear()
        wp = pre_left
        while wp and wp.lane_type == carla.LaneType.Driving:
            if wp.lane_id in lane_id_set:
                break
            lane_id_set.add(wp.lane_id)

            lane_list.append(wp)

            # carla bug: Return error, never return none, endless loop
            wp = wp.get_right_lane()

        return lane_list

    @staticmethod
    def get_road_lane_cnt(wp):

    # cspell:ignore rolename
    @staticmethod
    def create_blueprint(
        model, rolename="scenario", color=None, actor_category="car", attribute_filter=None
    ):
        """Set up the blueprint of an actor given its model and other relevant parameters."""

        def check_attribute_value(blueprint, name, value):
            """Check if the blueprint has that attribute with that value."""
            if not blueprint.has_attribute(name):
                return False

            attribute_type = blueprint.get_attribute(key).type
            if attribute_type == carla.ActorAttributeType.Bool:
                return blueprint.get_attribute(name).as_bool() == value
            elif attribute_type == carla.ActorAttributeType.Int:
                return blueprint.get_attribute(name).as_int() == value
            elif attribute_type == carla.ActorAttributeType.Float:
                return blueprint.get_attribute(name).as_float() == value
            elif attribute_type == carla.ActorAttributeType.String:
                return blueprint.get_attribute(name).as_str() == value

            return False

        # cspell:ignore carlacola carlamotors
        _actor_blueprint_categories = {
            "car": "vehicle.tesla.model3",
            "van": "vehicle.volkswagen.t2",
            "truck": "vehicle.carlamotors.carlacola",
            "trailer": "",
            "semitrailer": "",
            "bus": "vehicle.volkswagen.t2",
            "motorbike": "vehicle.kawasaki.ninja",
            "bicycle": "vehicle.diamondback.century",
            "train": "",
            "tram": "",
            "pedestrian": "walker.pedestrian.0001",
        }

        # Set the model
        try:
            blueprints = CarlaDataProvider._blueprint_library.filter(model)
            if attribute_filter is not None:
                for key, value in attribute_filter.items():
                    blueprints = [x for x in blueprints if check_attribute_value(x, key, value)]

            blueprint = CarlaDataProvider._rng.choice(blueprints)
        except ValueError:
            # The model is not part of the blueprint library. Let's take a default one for the given category
            bp_filter = "vehicle.*"
            new_model = _actor_blueprint_categories[actor_category]
            if new_model != "":
                bp_filter = new_model
            print(
                "WARNING: Actor model {} not available. Using instead {}".format(model, new_model)
            )
            blueprint = CarlaDataProvider._rng.choice(
                CarlaDataProvider._blueprint_library.filter(bp_filter)
            )

        # Set the color
        if color:
            if not blueprint.has_attribute("color"):
                print(
                    "WARNING: Cannot set Color ({}) for actor {} due to missing blueprint attribute".format(
                        color, blueprint.id
                    )
                )
            else:
                default_color_rgba = blueprint.get_attribute("color").as_color()
                default_color = "({}, {}, {})".format(
                    default_color_rgba.r, default_color_rgba.g, default_color_rgba.b
                )
                try:
                    blueprint.set_attribute("color", color)
                except ValueError:
                    # Color can't be set for this vehicle
                    print(
                        "WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format(
                            color, blueprint.id, default_color
                        )
                    )
                    blueprint.set_attribute("color", default_color)
        else:
            if blueprint.has_attribute("color") and rolename != "hero":
                color = CarlaDataProvider._rng.choice(
                    blueprint.get_attribute("color").recommended_values
                )
                blueprint.set_attribute("color", color)

        # Make pedestrians mortal
        if blueprint.has_attribute("is_invincible"):
            blueprint.set_attribute("is_invincible", "false")

        # Set the rolename
        if blueprint.has_attribute("role_name"):
            blueprint.set_attribute("role_name", rolename)

        return blueprint

    @staticmethod
    def handle_actor_batch(batch, tick=True):
        """Forward a CARLA command batch to spawn actors to CARLA, and gather the responses."""
        sync_mode = CarlaDataProvider.is_sync_mode()
        actors = []

        if CarlaDataProvider._client:
            responses = CarlaDataProvider._client.apply_batch_sync(batch, sync_mode and tick)
        else:
            raise ValueError("class member 'client'' not initialized yet")

        # Wait (or not) for the actors to be spawned properly before we do anything
        if not tick:
            pass
        elif CarlaDataProvider.is_runtime_init_mode():
            CarlaDataProvider._world.wait_for_tick()
        elif sync_mode:
            CarlaDataProvider._world.tick()
        else:
            CarlaDataProvider._world.wait_for_tick()

        actor_ids = [r.actor_id for r in responses if not r.error]
        for r in responses:
            if r.error:
                print("WARNING: Not all actors were spawned")
                break
        actors = list(CarlaDataProvider._world.get_actors(actor_ids))
        return actors

    @staticmethod
    def request_new_actor(
        model,
        spawn_point,
        rolename="scenario",
        autopilot=False,
        random_location=False,
        color=None,
        actor_category="car",
        attribute_filter=None,
        tick=True,
    ):
        """Create a new actor, returning it if successful (None otherwise)."""
        blueprint = CarlaDataProvider.create_blueprint(
            model, rolename, color, actor_category, attribute_filter
        )

        if random_location:
            actor = None
            while not actor:
                spawn_point = CarlaDataProvider._rng.choice(CarlaDataProvider._spawn_points)
                actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point)

        else:
            # For non prop models, slightly lift the actor to avoid collisions with the ground
            z_offset = 0.2 if "prop" not in model else 0

            # DO NOT USE spawn_point directly, as this will modify spawn_point permanently
            _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation)
            _spawn_point.location.x = spawn_point.location.x
            _spawn_point.location.y = spawn_point.location.y
            _spawn_point.location.z = spawn_point.location.z + z_offset
            actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point)

        if actor is None:
            print(
                "WARNING: Cannot spawn actor {} at position {}".format(model, spawn_point.location)
            )
            return None

        # De/activate the autopilot of the actor if it belongs to vehicle
        if autopilot:
            if isinstance(actor, carla.Vehicle):
                actor.set_autopilot(autopilot, CarlaDataProvider._traffic_manager_port)
            else:
                print("WARNING: Tried to set the autopilot of a non vehicle actor")

        # Wait for the actor to be spawned properly before we do anything
        if not tick:
            pass
        elif CarlaDataProvider.is_runtime_init_mode():
            CarlaDataProvider._world.wait_for_tick()
        elif CarlaDataProvider.is_sync_mode():
            CarlaDataProvider._world.tick()
        else:
            CarlaDataProvider._world.wait_for_tick()

        if actor is None:
            return None

        CarlaDataProvider._carla_actor_pool[actor.id] = actor
        CarlaDataProvider.register_actor(actor, spawn_point)
        return actor

    @staticmethod
    def request_new_actors(actor_list, attribute_filter=None, tick=True):
        """Series of actor in batch. If this was successful, the new actors are returned, None otherwise."""
        SpawnActor = carla.command.SpawnActor  # pylint: disable=invalid-name
        PhysicsCommand = carla.command.SetSimulatePhysics  # pylint: disable=invalid-name
        FutureActor = carla.command.FutureActor  # pylint: disable=invalid-name
        ApplyTransform = carla.command.ApplyTransform  # pylint: disable=invalid-name
        SetAutopilot = carla.command.SetAutopilot  # pylint: disable=invalid-name
        SetVehicleLightState = carla.command.SetVehicleLightState  # pylint: disable=invalid-name

        batch = []
        actors = []

        CarlaDataProvider.generate_spawn_points()

        for actor in actor_list:
            # Get the blueprint
            blueprint = CarlaDataProvider.create_blueprint(
                actor.model, actor.rolename, actor.color, actor.category, attribute_filter
            )

            # Get the spawn point
            transform = actor.transform
            if not transform:
                continue
            if actor.random_location:
                if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points):
                    print("No more spawn points to use")
                    break
                else:
                    _spawn_point = CarlaDataProvider._spawn_points[
                        CarlaDataProvider._spawn_index
                    ]  # pylint: disable=unsubscriptable-object
                    CarlaDataProvider._spawn_index += 1

            else:
                _spawn_point = carla.Transform()
                _spawn_point.rotation = transform.rotation
                _spawn_point.location.x = transform.location.x
                _spawn_point.location.y = transform.location.y
                if blueprint.has_tag("walker"):
                    # On imported OpenDRIVE maps, spawning of pedestrians can fail.
                    # By increasing the z-value the chances of success are increased.
                    map_name = CarlaDataProvider._map.name.split("/")[-1]
                    if not map_name.startswith("OpenDrive"):
                        _spawn_point.location.z = transform.location.z + 0.2
                    else:
                        _spawn_point.location.z = transform.location.z + 0.8
                else:
                    _spawn_point.location.z = transform.location.z + 0.2

            # Get the command
            command = SpawnActor(blueprint, _spawn_point)
            command.then(
                SetAutopilot(FutureActor, actor.autopilot, CarlaDataProvider._traffic_manager_port)
            )

            if (
                actor.args is not None
                and "physics" in actor.args
                and actor.args["physics"] == "off"
            ):
                command.then(ApplyTransform(FutureActor, _spawn_point)).then(
                    PhysicsCommand(FutureActor, False)
                )
            elif actor.category == "misc":
                command.then(PhysicsCommand(FutureActor, True))
            if actor.args is not None and "lights" in actor.args and actor.args["lights"] == "on":
                command.then(SetVehicleLightState(FutureActor, carla.VehicleLightState.All))

            batch.append(command)

        actors = CarlaDataProvider.handle_actor_batch(batch, tick)

        if not actors:
            return None

        for actor in actors:
            if actor is None:
                continue
            CarlaDataProvider._carla_actor_pool[actor.id] = actor
            CarlaDataProvider.register_actor(actor, _spawn_point)

        return actors

    @staticmethod
    def request_new_batch_actors(
        model,
        amount,
        spawn_points,
        autopilot=False,
        random_location=False,
        rolename="scenario",
        attribute_filter=None,
        tick=True,
    ):
        SpawnActor = carla.command.SpawnActor  # pylint: disable=invalid-name
        SetAutopilot = carla.command.SetAutopilot  # pylint: disable=invalid-name
        FutureActor = carla.command.FutureActor  # pylint: disable=invalid-name

        CarlaDataProvider.generate_spawn_points()

        batch = []

        for i in range(amount):
            # Get vehicle by model
            blueprint = CarlaDataProvider.create_blueprint(
                model, rolename, attribute_filter=attribute_filter
            )

            if random_location:
                if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points):
                    print(
                        "No more spawn points to use. Spawned {} actors out of {}".format(
                            i + 1, amount
                        )
                    )
                    break
                else:
                    spawn_point = CarlaDataProvider._spawn_points[
                        CarlaDataProvider._spawn_index
                    ]  # pylint: disable=unsubscriptable-object
                    CarlaDataProvider._spawn_index += 1
            else:
                try:
                    spawn_point = spawn_points[i]
                except IndexError:
                    print("The amount of spawn points is lower than the amount of vehicles spawned")
                    break

            if spawn_point:
                batch.append(
                    SpawnActor(blueprint, spawn_point).then(
                        SetAutopilot(
                            FutureActor, autopilot, CarlaDataProvider._traffic_manager_port
                        )
                    )
                )

        actors = CarlaDataProvider.handle_actor_batch(batch, tick)
        for actor in actors:
            if actor is None:
                continue
            CarlaDataProvider._carla_actor_pool[actor.id] = actor
            CarlaDataProvider.register_actor(actor, spawn_point)

        return actors

    @staticmethod
    def get_actors():
        CarlaDataProvider._traffic_manager_port = tm_port

    @staticmethod
    def cleanup():
        """Cleanup and remove all entries from all dictionaries."""
        DestroyActor = carla.command.DestroyActor  # pylint: disable=invalid-name
        batch = []

        for actor_id in CarlaDataProvider._carla_actor_pool.copy():
            actor = CarlaDataProvider._carla_actor_pool[actor_id]
            if actor is not None and actor.is_alive:
                batch.append(DestroyActor(actor))

        if CarlaDataProvider._client:
            try:
                CarlaDataProvider._client.apply_batch_sync(batch)
            except RuntimeError as e:
                if "time-out" in str(e):
                    pass
                else:
                    raise e

        CarlaDataProvider._actor_velocity_map.clear()
        CarlaDataProvider._actor_location_map.clear()
        CarlaDataProvider._actor_transform_map.clear()
        CarlaDataProvider._traffic_light_map.clear()
        CarlaDataProvider._map = None
        CarlaDataProvider._world = None
        CarlaDataProvider._sync_flag = False
        CarlaDataProvider._ego_vehicle_route = None
        CarlaDataProvider._all_actors = None
        CarlaDataProvider._carla_actor_pool = {}
        CarlaDataProvider._client = None
        CarlaDataProvider._spawn_points = None
        CarlaDataProvider._spawn_index = 0
        CarlaDataProvider._rng = random.RandomState(CarlaDataProvider._random_seed)
        CarlaDataProvider._runtime_init_flag = False

    @property
    def world(self):
diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py
    def __call__(self):
        return self._agent()

    def setup_sensors(self, vehicle, debug_mode=False):
        """Create and attach the sensor defined in objects.json."""
        bp_library = CarlaDataProvider.get_world().get_blueprint_library()

        for sensor_spec in self._agent.sensors["sensors"]:
            bp = bp_library.find(str(sensor_spec["type"]))

            if sensor_spec["type"].startswith("sensor.camera"):
                bp.set_attribute("image_size_x", str(sensor_spec["image_size_x"]))
                bp.set_attribute("image_size_y", str(sensor_spec["image_size_y"]))
                bp.set_attribute("fov", str(sensor_spec["fov"]))
                sensor_location = carla.Location(
                    x=sensor_spec["spawn_point"]["x"],
                    y=sensor_spec["spawn_point"]["y"],
                    z=sensor_spec["spawn_point"]["z"],
                )
                sensor_rotation = carla.Rotation(
                    pitch=sensor_spec["spawn_point"]["pitch"],
                    roll=sensor_spec["spawn_point"]["roll"],
                    yaw=sensor_spec["spawn_point"]["yaw"],
                )

            elif sensor_spec["type"].startswith("sensor.lidar"):
                bp.set_attribute("range", str(sensor_spec["range"]))
                bp.set_attribute("rotation_frequency", str(sensor_spec["rotation_frequency"]))
                bp.set_attribute("channels", str(sensor_spec["channels"]))
                bp.set_attribute("upper_fov", str(sensor_spec["upper_fov"]))
                bp.set_attribute("lower_fov", str(sensor_spec["lower_fov"]))
                bp.set_attribute("points_per_second", str(sensor_spec["points_per_second"]))
                sensor_location = carla.Location(
                    x=sensor_spec["spawn_point"]["x"],
                    y=sensor_spec["spawn_point"]["y"],
                    z=sensor_spec["spawn_point"]["z"],
                )
                sensor_rotation = carla.Rotation(
                    pitch=sensor_spec["spawn_point"]["pitch"],
                    roll=sensor_spec["spawn_point"]["roll"],
                    yaw=sensor_spec["spawn_point"]["yaw"],
                )

            elif sensor_spec["type"].startswith("sensor.other.gnss"):
                bp.set_attribute("noise_alt_stddev", str(0.0))
                bp.set_attribute("noise_lat_stddev", str(0.0))
                bp.set_attribute("noise_lon_stddev", str(0.0))
                bp.set_attribute("noise_alt_bias", str(0.0))
                bp.set_attribute("noise_lat_bias", str(0.0))
                bp.set_attribute("noise_lon_bias", str(0.0))
                sensor_location = carla.Location(
                    x=sensor_spec["spawn_point"]["x"],
                    y=sensor_spec["spawn_point"]["y"],
                    z=sensor_spec["spawn_point"]["z"],
                )
                sensor_rotation = carla.Rotation(
                    pitch=sensor_spec["spawn_point"]["pitch"],
                    roll=sensor_spec["spawn_point"]["roll"],
                    yaw=sensor_spec["spawn_point"]["yaw"],
                )

            elif sensor_spec["type"].startswith("sensor.other.imu"):
                bp.set_attribute("noise_accel_stddev_x", str(0.0))
                bp.set_attribute("noise_accel_stddev_y", str(0.0))
                bp.set_attribute("noise_accel_stddev_z", str(0.0))
                bp.set_attribute("noise_gyro_stddev_x", str(0.0))
                bp.set_attribute("noise_gyro_stddev_y", str(0.0))
                bp.set_attribute("noise_gyro_stddev_z", str(0.0))
                sensor_location = carla.Location(
                    x=sensor_spec["spawn_point"]["x"],
                    y=sensor_spec["spawn_point"]["y"],
                    z=sensor_spec["spawn_point"]["z"],
                )
                sensor_rotation = carla.Rotation(
                    pitch=sensor_spec["spawn_point"]["pitch"],
                    roll=sensor_spec["spawn_point"]["roll"],
                    yaw=sensor_spec["spawn_point"]["yaw"],
                )

            elif sensor_spec["type"].startswith("sensor.pseudo.*"):
                sensor_location = carla.Location(
                    x=sensor_spec["spawn_point"]["x"],
                    y=sensor_spec["spawn_point"]["y"],
                    z=sensor_spec["spawn_point"]["z"],
                )
                sensor_rotation = carla.Rotation(
                    pitch=sensor_spec["spawn_point"]["pitch"] + 0.001,
                    roll=sensor_spec["spawn_point"]["roll"] - 0.015,
                    yaw=sensor_spec["spawn_point"]["yaw"] + 0.0364,
                )

            # create sensor
            sensor_transform = carla.Transform(sensor_location, sensor_rotation)
            sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle)
            # setup callback
            sensor.listen(CallBack(sensor_spec["id"], sensor, self._agent.sensor_interface))
            self._sensors_list.append(sensor)

        # Tick once to spawn the sensors
        CarlaDataProvider.get_world().tick()

    def cleanup(self):
        """Cleanup sensors."""
	self.use_traffic_manager = self.param_["use_traffic_manager"]
	self.max_real_delta_seconds = self.param_["max_real_delta_seconds"]

	def load_world(self):
	client = carla.Client(self.local_host, self.port)
	client.set_timeout(self.timeout)
	client.load_world(self.carla_map)
	self.world = client.get_world()
	settings = self.world.get_settings()
	settings.fixed_delta_seconds = self.fixed_delta_seconds
	settings.synchronous_mode = self.sync_mode
	self.world.apply_settings(settings)
	CarlaDataProvider.set_world(self.world)
	CarlaDataProvider.set_client(client)
	spawn_point = carla.Transform()
	point_items = self.spawn_point.split(",")
	randomize = False
	if len(point_items) == 6:
	spawn_point.location.x = float(point_items[0])
	spawn_point.location.y = float(point_items[1])
	spawn_point.location.z = (
	float(point_items[2]) + 2
	) # +2 is used so the car did not stuck on the road when spawned.
	spawn_point.rotation.roll = float(point_items[3])
	spawn_point.rotation.pitch = float(point_items[4])
	spawn_point.rotation.yaw = float(point_items[5])
	else:
	randomize = True
	self.ego_actor = CarlaDataProvider.request_new_actor(
	self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize
	)
	self.interface.ego_actor = self.ego_actor # TODO improve design
	self.interface.physics_control = self.ego_actor.get_physics_control()

	self.sensor_wrapper = SensorWrapper(self.interface)
	self.sensor_wrapper.setup_sensors(self.ego_actor, False)
	##########################################################################################################################################################
	# TRAFFIC MANAGER
	##########################################################################################################################################################
	# cspell:ignore trafficmanager
	if self.use_traffic_manager:
	traffic_manager = client.get_trafficmanager()
	traffic_manager.set_synchronous_mode(True)
	traffic_manager.set_random_device_seed(0)
	random.seed(0)
	spawn_points_tm = self.world.get_map().get_spawn_points()
	for i, spawn_point in enumerate(spawn_points_tm):
	self.world.debug.draw_string(spawn_point.location, str(i), life_time=10)
	models = [
	"dodge",
	"audi",
	"model3",
	"mini",
	"mustang",
	"lincoln",
	"prius",
	"nissan",
	"crown",
	"impala",
	]
	blueprints = []
	for vehicle in self.world.get_blueprint_library().filter("vehicle"):
	if any(model in vehicle.id for model in models):
	blueprints.append(vehicle)
	max_vehicles = 30
	max_vehicles = min([max_vehicles, len(spawn_points_tm)])
	vehicles = []
	for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)):
	temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point)
	if temp is not None:
	vehicles.append(temp)

	for vehicle in vehicles:
	vehicle.set_autopilot(True)
Check warning on line 147 in simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Complex Method `InitializeInterface.load_world has a cyclomatic complexity of 11, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.` Check warning on line 147 in simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Bumpy Road Ahead `InitializeInterface.load_world has 3 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.`

	def run_bridge(self):
	self.bridge_loop = SensorLoop()
	self.bridge_loop.sensor = self.sensor_wrapper
	self.bridge_loop.ego_actor = self.ego_actor
	self.bridge_loop.start_system_time = time.time()
	self.bridge_loop.start_game_time = GameTime.get_time()
	self.bridge_loop.running = True
	while self.bridge_loop.running:
	timestamp = None
	world = CarlaDataProvider.get_world()
	if world:
	snapshot = world.get_snapshot()
	if snapshot:
	timestamp = snapshot.timestamp
	if timestamp:
	delta_step = time.time() - self.prev_tick_wall_time
	if delta_step <= self.max_real_delta_seconds:
	# Add a wait to match the max_real_delta_seconds
	time.sleep(self.max_real_delta_seconds - delta_step)
	self.prev_tick_wall_time = time.time()
	self.bridge_loop._tick_sensor(timestamp)
Check warning on line 169 in simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Bumpy Road Ahead `InitializeInterface.run_bridge has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.`

	def _stop_loop(self, sign, frame):
	self.bridge_loop._stop_loop()


	class carla_ros2_interface(object):
	def __init__(self):
	self.sensor_interface = SensorInterface()
	self.timestamp = None
	self.ego_actor = None
	self.physics_control = None
	self.channels = 0
	self.id_to_sensor_type_map = {}
	self.id_to_camera_info_map = {}
	self.cv_bridge = CvBridge()
	self.first_ = True
	self.pub_lidar = {}
	self.sensor_frequencies = {
	"top": 11,
	"left": 11,
	"right": 11,
	"camera": 11,
	"imu": 50,
	"status": 50,
	"pose": 2,
	}
	self.publish_prev_times = {
	sensor: datetime.datetime.now() for sensor in self.sensor_frequencies
	}

	# initialize ros2 node
	rclpy.init(args=None)
	self.ros2_node = rclpy.create_node("carla_ros2_interface")
	self.parameters = {
	"host": rclpy.Parameter.Type.STRING,
	"port": rclpy.Parameter.Type.INTEGER,
	"sync_mode": rclpy.Parameter.Type.BOOL,
	"timeout": rclpy.Parameter.Type.INTEGER,
	"fixed_delta_seconds": rclpy.Parameter.Type.DOUBLE,
	"carla_map": rclpy.Parameter.Type.STRING,
	"ego_vehicle_role_name": rclpy.Parameter.Type.STRING,
	"spawn_point": rclpy.Parameter.Type.STRING,
	"vehicle_type": rclpy.Parameter.Type.STRING,
	"objects_definition_file": rclpy.Parameter.Type.STRING,
	"use_traffic_manager": rclpy.Parameter.Type.BOOL,
	"max_real_delta_seconds": rclpy.Parameter.Type.DOUBLE,
	}
	self.param_values = {}
	for param_name, param_type in self.parameters.items():
	self.ros2_node.declare_parameter(param_name, param_type)
	self.param_values[param_name] = self.ros2_node.get_parameter(param_name).value

	# Publish clock
	self.clock_publisher = self.ros2_node.create_publisher(Clock, "/clock", 10)
	obj_clock = Clock()
	obj_clock.clock = Time(sec=0)
	self.clock_publisher.publish(obj_clock)

	# Sensor Config (Edit your sensor here)
	self.sensors = json.load(open(self.param_values["objects_definition_file"]))

	# Subscribing Autoware Control messages and converting to CARLA control
	self.sub_control = self.ros2_node.create_subscription(
	ActuationCommandStamped, "/control/command/actuation_cmd", self.control_callback, 1
	)

	self.sub_vehicle_initialpose = self.ros2_node.create_subscription(
	PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1
	)

	self.current_control = carla.VehicleControl()

	# Direct data publishing from CARLA for Autoware
	self.pub_pose_with_cov = self.ros2_node.create_publisher(
	PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1
	)
	self.pub_vel_state = self.ros2_node.create_publisher(
	VelocityReport, "/vehicle/status/velocity_status", 1
	)
	self.pub_steering_state = self.ros2_node.create_publisher(
	SteeringReport, "/vehicle/status/steering_status", 1
	)
	self.pub_ctrl_mode = self.ros2_node.create_publisher(
	ControlModeReport, "/vehicle/status/control_mode", 1
	)
	self.pub_gear_state = self.ros2_node.create_publisher(
	GearReport, "/vehicle/status/gear_status", 1
	)
	self.pub_actuation_status = self.ros2_node.create_publisher(
	ActuationStatusStamped, "/vehicle/status/actuation_status", 1
	)

	# Create Publisher for each Physical Sensors
	for sensor in self.sensors["sensors"]:
	self.id_to_sensor_type_map[sensor["id"]] = sensor["type"]
	if sensor["type"] == "sensor.camera.rgb":
	self.pub_camera = self.ros2_node.create_publisher(
	Image, "/sensing/camera/traffic_light/image_raw", 1
	)
	self.pub_camera_info = self.ros2_node.create_publisher(
	CameraInfo, "/sensing/camera/traffic_light/camera_info", 1
	)
	elif sensor["type"] == "sensor.lidar.ray_cast":
	if sensor["id"] in self.sensor_frequencies:
	self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher(
	PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10
	)
	else:
	self.ros2_node.get_logger().info(
	"Please use Top, Right, or Left as the LIDAR ID"
	)
	elif sensor["type"] == "sensor.other.imu":
	self.pub_imu = self.ros2_node.create_publisher(
	Imu, "/sensing/imu/tamagawa/imu_raw", 1
	)
	else:
	self.ros2_node.get_logger().info(f'No Publisher for {sensor["type"]} Sensor')
	pass

	self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,))
	self.spin_thread.start()
Check warning on line 165 in simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Complex Method `carla_ros2_interface.__init__ has a cyclomatic complexity of 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.` Check warning on line 165 in simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Bumpy Road Ahead `carla_ros2_interface.__init__ has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.`

	def __call__(self):
	input_data = self.sensor_interface.get_data()
	def __call__(self):
	return self._agent()

	def setup_sensors(self, vehicle, debug_mode=False):
	"""Create and attach the sensor defined in objects.json."""
	bp_library = CarlaDataProvider.get_world().get_blueprint_library()

	for sensor_spec in self._agent.sensors["sensors"]:
	bp = bp_library.find(str(sensor_spec["type"]))

	if sensor_spec["type"].startswith("sensor.camera"):
	bp.set_attribute("image_size_x", str(sensor_spec["image_size_x"]))
	bp.set_attribute("image_size_y", str(sensor_spec["image_size_y"]))
	bp.set_attribute("fov", str(sensor_spec["fov"]))
	sensor_location = carla.Location(
	x=sensor_spec["spawn_point"]["x"],
	y=sensor_spec["spawn_point"]["y"],
	z=sensor_spec["spawn_point"]["z"],
	)
	sensor_rotation = carla.Rotation(
	pitch=sensor_spec["spawn_point"]["pitch"],
	roll=sensor_spec["spawn_point"]["roll"],
	yaw=sensor_spec["spawn_point"]["yaw"],
	)

	elif sensor_spec["type"].startswith("sensor.lidar"):
	bp.set_attribute("range", str(sensor_spec["range"]))
	bp.set_attribute("rotation_frequency", str(sensor_spec["rotation_frequency"]))
	bp.set_attribute("channels", str(sensor_spec["channels"]))
	bp.set_attribute("upper_fov", str(sensor_spec["upper_fov"]))
	bp.set_attribute("lower_fov", str(sensor_spec["lower_fov"]))
	bp.set_attribute("points_per_second", str(sensor_spec["points_per_second"]))
	sensor_location = carla.Location(
	x=sensor_spec["spawn_point"]["x"],
	y=sensor_spec["spawn_point"]["y"],
	z=sensor_spec["spawn_point"]["z"],
	)
	sensor_rotation = carla.Rotation(
	pitch=sensor_spec["spawn_point"]["pitch"],
	roll=sensor_spec["spawn_point"]["roll"],
	yaw=sensor_spec["spawn_point"]["yaw"],
	)

	elif sensor_spec["type"].startswith("sensor.other.gnss"):
	bp.set_attribute("noise_alt_stddev", str(0.0))
	bp.set_attribute("noise_lat_stddev", str(0.0))
	bp.set_attribute("noise_lon_stddev", str(0.0))
	bp.set_attribute("noise_alt_bias", str(0.0))
	bp.set_attribute("noise_lat_bias", str(0.0))
	bp.set_attribute("noise_lon_bias", str(0.0))
	sensor_location = carla.Location(
	x=sensor_spec["spawn_point"]["x"],
	y=sensor_spec["spawn_point"]["y"],
	z=sensor_spec["spawn_point"]["z"],
	)
	sensor_rotation = carla.Rotation(
	pitch=sensor_spec["spawn_point"]["pitch"],
	roll=sensor_spec["spawn_point"]["roll"],
	yaw=sensor_spec["spawn_point"]["yaw"],
	)

	elif sensor_spec["type"].startswith("sensor.other.imu"):
	bp.set_attribute("noise_accel_stddev_x", str(0.0))
	bp.set_attribute("noise_accel_stddev_y", str(0.0))
	bp.set_attribute("noise_accel_stddev_z", str(0.0))
	bp.set_attribute("noise_gyro_stddev_x", str(0.0))
	bp.set_attribute("noise_gyro_stddev_y", str(0.0))
	bp.set_attribute("noise_gyro_stddev_z", str(0.0))
	sensor_location = carla.Location(
	x=sensor_spec["spawn_point"]["x"],
	y=sensor_spec["spawn_point"]["y"],
	z=sensor_spec["spawn_point"]["z"],
	)
	sensor_rotation = carla.Rotation(
	pitch=sensor_spec["spawn_point"]["pitch"],
	roll=sensor_spec["spawn_point"]["roll"],
	yaw=sensor_spec["spawn_point"]["yaw"],
	)

	elif sensor_spec["type"].startswith("sensor.pseudo.*"):
	sensor_location = carla.Location(
	x=sensor_spec["spawn_point"]["x"],
	y=sensor_spec["spawn_point"]["y"],
	z=sensor_spec["spawn_point"]["z"],
	)
	sensor_rotation = carla.Rotation(
	pitch=sensor_spec["spawn_point"]["pitch"] + 0.001,
	roll=sensor_spec["spawn_point"]["roll"] - 0.015,
	yaw=sensor_spec["spawn_point"]["yaw"] + 0.0364,
	)

	# create sensor
	sensor_transform = carla.Transform(sensor_location, sensor_rotation)
	sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle)
	# setup callback
	sensor.listen(CallBack(sensor_spec["id"], sensor, self._agent.sensor_interface))
	self._sensors_list.append(sensor)

	# Tick once to spawn the sensors
	CarlaDataProvider.get_world().tick()
Check warning on line 222 in simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Large Method `SensorWrapper.setup_sensors has 86 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.`

	def cleanup(self):
	"""Cleanup sensors."""