From bf0a4b00cd91972df1634b6a879ab759b5fa36b7 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 31 May 2024 17:08:19 +0900 Subject: [PATCH] fix steering and small cleanup of the interface Signed-off-by: Maxime CLEMENT --- .../raw_vehicle_cmd_converter.param.yaml | 2 +- .../launch/carla_autoware.launch.xml | 88 ++++++---- .../src/carla_autoware/carla_autoware.py | 156 +++++++++--------- .../src/carla_autoware/carla_ros.py | 75 +++++---- 4 files changed, 169 insertions(+), 152 deletions(-) diff --git a/simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml b/simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml index 6d20cf0e79c3c..48fd451c3a8f9 100644 --- a/simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml +++ b/simulator/carla_autoware/config/raw_vehicle_cmd_converter.param.yaml @@ -5,7 +5,7 @@ csv_path_steer_map: $(find-pkg-share carla_autoware)/steer_map.csv convert_accel_cmd: true convert_brake_cmd: true - convert_steer_cmd: true + convert_steer_cmd: false use_steer_ff: true use_steer_fb: true is_debugging: false diff --git a/simulator/carla_autoware/launch/carla_autoware.launch.xml b/simulator/carla_autoware/launch/carla_autoware.launch.xml index e24014640a411..ed38e2278239e 100644 --- a/simulator/carla_autoware/launch/carla_autoware.launch.xml +++ b/simulator/carla_autoware/launch/carla_autoware.launch.xml @@ -1,40 +1,56 @@ - - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - + + + + + + + + + + + + + + + + + + + diff --git a/simulator/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py index c494092aebd15..f12ced6900e58 100644 --- a/simulator/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py @@ -26,15 +26,14 @@ class SensorLoop(object): - def __init__(self, role_name): + def __init__(self): self.start_game_time = None self.start_system_time = None self.sensor = None - self.ego_vehicle = None + self.ego_actor = None self.running = False self.timestamp_last_run = 0.0 self.timeout = 20.0 - self.role_name = role_name def _stop_loop(self): self.running = False @@ -48,7 +47,7 @@ def _tick_sensor(self, timestamp): ego_action = self.sensor() except SensorReceivedNoData as e: raise RuntimeError(e) - self.ego_vehicle.apply_control(ego_action) + self.ego_actor.apply_control(ego_action) if self.running: CarlaDataProvider.get_world().tick() @@ -59,7 +58,7 @@ def __init__(self): self.param_ = self.interface.get_param() self.world = None self.sensor_wrapper = None - self.ego_vehicle = None + self.ego_actor = None self.prev_tick_wall_time = 0.0 # Parameter for Initializing Carla World @@ -80,83 +79,78 @@ def load_world(self): client.set_timeout(self.timeout) client.load_world(self.carla_map) self.world = client.get_world() - if self.world is not None: - 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() - 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 - CarlaDataProvider.request_new_actor( - self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize - ) - - self.sensor_wrapper = SensorWrapper(self.interface) - self.ego_vehicle = CarlaDataProvider.get_actor_by_name(self.agent_role_name) - self.sensor_wrapper.setup_sensors(self.ego_vehicle, False) - ########################################################################################################################################################## - # TRAFFIC MANAGER - ########################################################################################################################################################## - 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) - + 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: - print("Carla Interface Couldn't find the world, Carla is not Running") + 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 + ########################################################################################################################################################## + 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.agent_role_name) + self.bridge_loop = SensorLoop() self.bridge_loop.sensor = self.sensor_wrapper - self.bridge_loop.ego_vehicle = self.ego_vehicle + 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.role_name = self.agent_role_name self.bridge_loop.running = True while self.bridge_loop.running: timestamp = None @@ -179,20 +173,20 @@ def _stop_loop(self, signum, frame): def _cleanup(self): self.sensor_wrapper.cleanup() CarlaDataProvider.cleanup() - if self.ego_vehicle: - self.ego_vehicle.destroy() - self.ego_vehicle = None + if self.ego_actor: + self.ego_actor.destroy() + self.ego_actor = None if self.interface: + self.interface.shutdown() self.interface = None def main(): carla_bridge = InitializeInterface() carla_bridge.load_world() - stop_bridge = signal.signal(signal.SIGINT, carla_bridge._stop_loop) + signal.signal(signal.SIGINT, carla_bridge._stop_loop) carla_bridge.run_bridge() - signal.signal(signal.SIGINT, stop_bridge) carla_bridge._cleanup() diff --git a/simulator/carla_autoware/src/carla_autoware/carla_ros.py b/simulator/carla_autoware/src/carla_autoware/carla_ros.py index c5328e7045a62..965a707c66cb4 100644 --- a/simulator/carla_autoware/src/carla_autoware/carla_ros.py +++ b/simulator/carla_autoware/src/carla_autoware/carla_ros.py @@ -53,17 +53,15 @@ class carla_interface(object): def __init__(self): self.sensor_interface = SensorInterface() - self.setup() - - def setup(self): 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, @@ -73,7 +71,6 @@ def setup(self): "status": 50, "pose": 2, } - self.max_steering_angle = 45 self.publish_prev_times = { sensor: datetime.datetime.now() for sensor in self.sensor_frequencies } @@ -140,7 +137,7 @@ def setup(self): self.pub_gear_state = self.ros2_node.create_publisher( GearReport, "/vehicle/status/gear_status", 1 ) - self.actuation_status = self.ros2_node.create_publisher( + self.pub_actuation_status = self.ros2_node.create_publisher( ActuationStatusStamped, "/vehicle/status/actuation_status", 1 ) @@ -229,23 +226,13 @@ def lidar(self, carla_lidar_measurement, id_): point_cloud_msg = create_cloud(header, fields, lidar_data) self.pub_lidar[id_].publish(point_cloud_msg) - def get_ego_vehicle(self): - for car in CarlaDataProvider.get_world().get_actors().filter("vehicle.*"): - if car.attributes["role_name"] == self.agent_role_name: - return car - return None - def initialpose_callback(self, data): """Transform RVIZ initial pose to CARLA.""" - self.ego_vehicle = CarlaDataProvider.get_actor_by_name( - self.param_values["ego_vehicle_role_name"] - ) - pose = data.pose.pose pose.position.z += 2.0 carla_pose_transform = ros_pose_to_carla_transform(pose) - if self.ego_vehicle is not None: - self.ego_vehicle.set_transform(carla_pose_transform) + if self.ego_actor is not None: + self.ego_actor.set_transform(carla_pose_transform) else: print("Can't find Ego Vehicle") @@ -382,7 +369,17 @@ def control_callback(self, in_cmd): """Convert and publish CARLA Ego Vehicle Control to AUTOWARE.""" out_cmd = carla.VehicleControl() out_cmd.throttle = in_cmd.actuation.accel_cmd - out_cmd.steer = -in_cmd.actuation.steer_cmd + # convert base on steer curve of the vehicle + steer_curve = self.physics_control.steering_curve + current_vel = self.ego_actor.get_velocity() + max_steer_ratio = numpy.interp( + abs(current_vel.x), [v.x for v in steer_curve], [v.y for v in steer_curve] + ) + out_cmd.steer = ( + -in_cmd.actuation.steer_cmd + * max_steer_ratio + * math.radians(self.physics_control.wheels[0].max_steer_angle) + ) out_cmd.brake = in_cmd.actuation.brake_cmd self.current_control = out_cmd @@ -390,15 +387,21 @@ def ego_status(self): """Publish ego vehicle status.""" if self.checkFrequency("status"): return + self.publish_prev_times["status"] = datetime.datetime.now() - ego_vehicle = CarlaDataProvider.get_actor_by_name( - self.param_values["ego_vehicle_role_name"] - ) - control = ego_vehicle.get_control() - self.current_vel = CarlaDataProvider.get_velocity( - CarlaDataProvider.get_actor_by_name(self.param_values["ego_vehicle_role_name"]) - ) + # convert velocity from cartesian to ego frame + trans_mat = numpy.array(self.ego_actor.get_transform().get_matrix()).reshape(4, 4) + rot_mat = trans_mat[0:3, 0:3] + inv_rot_mat = rot_mat.T + vel_vec = numpy.array( + [ + self.ego_actor.get_velocity().x, + self.ego_actor.get_velocity().y, + self.ego_actor.get_velocity().z, + ] + ).reshape(3, 1) + ego_velocity = (inv_rot_mat @ vel_vec).T[0] out_vel_state = VelocityReport() out_steering_state = SteeringReport() @@ -408,13 +411,15 @@ def ego_status(self): out_actuation_status = ActuationStatusStamped() out_vel_state.header = self.get_msg_header(frame_id="base_link") - out_vel_state.longitudinal_velocity = self.current_vel - out_vel_state.lateral_velocity = 0.0 - out_vel_state.heading_rate = 0.0 + out_vel_state.longitudinal_velocity = ego_velocity[0] + out_vel_state.lateral_velocity = ego_velocity[1] + out_vel_state.heading_rate = ( + self.ego_actor.get_transform().transform_vector(self.ego_actor.get_angular_velocity()).z + ) out_steering_state.stamp = out_vel_state.header.stamp out_steering_state.steering_tire_angle = -math.radians( - ego_vehicle.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) + self.ego_actor.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) ) out_gear_state.stamp = out_vel_state.header.stamp @@ -423,14 +428,13 @@ def ego_status(self): out_ctrl_mode.stamp = out_vel_state.header.stamp out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS + control = self.ego_actor.get_control() out_actuation_status.header = self.get_msg_header(frame_id="base_link") out_actuation_status.status.accel_status = control.throttle out_actuation_status.status.brake_status = control.brake - out_actuation_status.status.steer_status = -math.radians( - ego_vehicle.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) - ) + out_actuation_status.status.steer_status = -control.steer - self.actuation_status.publish(out_actuation_status) + self.pub_actuation_status.publish(out_actuation_status) self.pub_vel_state.publish(out_vel_state) self.pub_steering_state.publish(out_steering_state) self.pub_ctrl_mode.publish(out_ctrl_mode) @@ -462,3 +466,6 @@ def run_step(self, input_data, timestamp): # Publish ego vehicle status self.ego_status() return self.current_control + + def shutdown(self): + self.ros2_node.destroy_node()