From 9908bfb083058f9603e14aef5c470feddb0b4c76 Mon Sep 17 00:00:00 2001 From: Minsu Kim Date: Thu, 23 May 2024 15:14:04 +0900 Subject: [PATCH 1/5] fix oscillation issue and apply carla's native ackermann control api --- .../src/carla_autoware/carla_autoware.py | 2 +- .../src/carla_autoware/carla_ros.py | 34 ++++++------------- 2 files changed, 12 insertions(+), 24 deletions(-) diff --git a/simulator/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py index c494092aebd15..b7c1ded0d24a5 100644 --- a/simulator/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py @@ -48,7 +48,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_vehicle.apply_ackermann_control(ego_action) if self.running: CarlaDataProvider.get_world().tick() diff --git a/simulator/carla_autoware/src/carla_autoware/carla_ros.py b/simulator/carla_autoware/src/carla_autoware/carla_ros.py index 2587800d793cc..48cbb365330a5 100644 --- a/simulator/carla_autoware/src/carla_autoware/carla_ros.py +++ b/simulator/carla_autoware/src/carla_autoware/carla_ros.py @@ -46,7 +46,6 @@ from .modules.carla_utils import carla_rotation_to_ros_quaternion from .modules.carla_utils import create_cloud from .modules.carla_utils import ros_pose_to_carla_transform -from .modules.carla_utils import steer_to_angle_map from .modules.carla_wrapper import SensorInterface @@ -119,7 +118,7 @@ def setup(self): PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1 ) - self.current_control = carla.VehicleControl() + self.current_control = carla.VehicleAckermannControl() # Direct data publishing from CARLA for Autoware self.pub_pose_with_cov = self.ros2_node.create_publisher( @@ -378,23 +377,13 @@ def imu(self, carla_imu_measurement): def control_callback(self, in_cmd): """Convert and publish CARLA Ego Vehicle Control to AUTOWARE.""" - out_cmd = carla.VehicleControl() - self.target_vel = in_cmd.longitudinal.speed - self.vel_diff = self.target_vel - self.current_vel - - if self.vel_diff > 0: - out_cmd.throttle = 0.75 - out_cmd.brake = 0.0 - elif self.vel_diff <= 0.0: - out_cmd.throttle = 0.0 - if self.target_vel <= 0.0: - out_cmd.brake = 0.75 - elif self.vel_diff > -1: - out_cmd.brake = 0.0 - else: - out_cmd.brake = 0.01 - - out_cmd.steer = -in_cmd.lateral.steering_tire_angle + out_cmd = carla.VehicleAckermannControl( + steer=numpy.clip(-math.degrees(in_cmd.lateral.steering_tire_angle)/self.max_steering_angle, -1.0, 1.0), + steer_speed=in_cmd.lateral.steering_tire_rotation_rate, + speed=in_cmd.longitudinal.speed, + acceleration=in_cmd.longitudinal.acceleration, + jerk=in_cmd.longitudinal.jerk + ) self.current_control = out_cmd def ego_status(self): @@ -406,7 +395,6 @@ def ego_status(self): self.param_values["ego_vehicle_role_name"] ) - in_status = ego_vehicle.get_control() self.current_vel = CarlaDataProvider.get_velocity( CarlaDataProvider.get_actor_by_name(self.param_values["ego_vehicle_role_name"]) ) @@ -423,10 +411,10 @@ def ego_status(self): out_vel_state.heading_rate = 0.0 out_steering_state.stamp = out_vel_state.header.stamp - out_steering_state.steering_tire_angle = steer_to_angle_map(self.max_steering_angle)( - in_status.steer + out_steering_state.steering_tire_angle = -math.radians( + ego_vehicle.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) ) - + out_gear_state.stamp = out_vel_state.header.stamp out_gear_state.report = GearReport.DRIVE From 36892499c25423159667d68335ee0e129ab6c4d4 Mon Sep 17 00:00:00 2001 From: Minsu Kim Date: Thu, 23 May 2024 15:16:29 +0900 Subject: [PATCH 2/5] fix perception issue by modifying lidar's spawning location --- simulator/carla_autoware/config/objects.json | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/simulator/carla_autoware/config/objects.json b/simulator/carla_autoware/config/objects.json index a240b8758321d..5dd9e2035deee 100644 --- a/simulator/carla_autoware/config/objects.json +++ b/simulator/carla_autoware/config/objects.json @@ -21,17 +21,17 @@ "spawn_point": { "x": 0.0, "y": 0.0, - "z": 2.6, + "z": 3.1, "roll": 0.0, "pitch": 0.0, "yaw": 0.0 }, - "range": 20, - "channels": 32, - "points_per_second": 200000, - "upper_fov": 3.0, - "lower_fov": -27.0, - "rotation_frequency": 20, + "range": 100, + "channels": 64, + "points_per_second": 1200000, + "upper_fov": 10.0, + "lower_fov": -30.0, + "rotation_frequency": 50, "noise_stddev": 0.0 }, { @@ -59,4 +59,4 @@ } } ] -} +} \ No newline at end of file From 3aa42cf2a31fbc1c5129fa2b62cfa71072150d3b Mon Sep 17 00:00:00 2001 From: Minsu Kim Date: Thu, 23 May 2024 15:16:49 +0900 Subject: [PATCH 3/5] sync README.md with the current implementation --- simulator/carla_autoware/README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/simulator/carla_autoware/README.md b/simulator/carla_autoware/README.md index 03d6ae7966bff..e6464d574ac8c 100644 --- a/simulator/carla_autoware/README.md +++ b/simulator/carla_autoware/README.md @@ -127,7 +127,6 @@ The `carla_ros.py` sets up the CARLA world: ## Tips - Misalignment might occurs during initialization, pressing `init by gnss` button should fix it. -- Changing the `fixed_delta_seconds` can increase the simulation tick (default 0.05 s), some sensors params in `objects.json` need to be adjusted when it is changed (example: LIDAR rotation frequency have to match the FPS). ## Known Issues and Future Works @@ -137,5 +136,3 @@ The `carla_ros.py` sets up the CARLA world: - Sensor currently not automatically configurated to have the same location as the Autoware Sensor kit. The current work around is to create a new frame of each sensors with (0, 0, 0, 0, 0, 0) coordinate relative to base_link and attach each sensor on the new frame (`carla_autoware.launch.xml` Line 28). This work around is very limited and restrictive, as when the sensor_kit is changed the sensor location will be wrongly attached. - Traffic light recognition. - Currently the HDmap of CARLA did not have information regarding the traffic light which is necessary for Autoware to conduct traffic light recognition. -- Ego Vehicle Control. - - Currently during sharp turning the ego-vehicle have large lateral error. From cf10e747f5042d966e23de9801d93e924ed23ac1 Mon Sep 17 00:00:00 2001 From: Minsu Kim Date: Thu, 23 May 2024 15:36:20 +0900 Subject: [PATCH 4/5] remove codes that won't be used anymore --- .../src/carla_autoware/modules/carla_utils.py | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/simulator/carla_autoware/src/carla_autoware/modules/carla_utils.py b/simulator/carla_autoware/src/carla_autoware/modules/carla_utils.py index 9def0c5d87dd1..01f981b53598a 100644 --- a/simulator/carla_autoware/src/carla_autoware/modules/carla_utils.py +++ b/simulator/carla_autoware/src/carla_autoware/modules/carla_utils.py @@ -108,16 +108,3 @@ def ros_pose_to_carla_transform(ros_pose): carla.Location(ros_pose.position.x, -ros_pose.position.y, ros_pose.position.z), ros_quaternion_to_carla_rotation(ros_pose.orientation), ) - - -def steer_to_angle_map(max_steering_angle): - """Compute the mapping from steering values to corresponding angles.""" - left_steer = -1 - right_steer = 1 - left_angle = np.radians(-max_steering_angle) - right_angle = -left_angle - steer_values = [left_steer, right_steer] - angle_values = [left_angle, right_angle] - coefficients = np.polyfit(steer_values, angle_values, 1) - mapping_function = np.poly1d(coefficients) - return mapping_function From f66488ad7590f0801063c8d9ffadd5d322f50f98 Mon Sep 17 00:00:00 2001 From: Minsu Kim Date: Thu, 23 May 2024 16:51:58 +0900 Subject: [PATCH 5/5] revert --- simulator/carla_autoware/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/simulator/carla_autoware/README.md b/simulator/carla_autoware/README.md index e6464d574ac8c..8c8839141a730 100644 --- a/simulator/carla_autoware/README.md +++ b/simulator/carla_autoware/README.md @@ -127,6 +127,7 @@ The `carla_ros.py` sets up the CARLA world: ## Tips - Misalignment might occurs during initialization, pressing `init by gnss` button should fix it. +- Changing the `fixed_delta_seconds` can increase the simulation tick (default 0.05 s), some sensors params in `objects.json` need to be adjusted when it is changed (example: LIDAR rotation frequency have to match the FPS). ## Known Issues and Future Works