Merge pull request #2 from Kim-mins/updated_version

fix: Fix issues on control & perception and improvement on handling control message.

simulator/carla_autoware/README.md

@@ -137,5 +137,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.

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 @@
       }
     }
   ]
-}
+}

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()
 

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
 

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