fixed performance and rearrange some packages

mraditya01 · Apr 12, 2024 · fc751af · fc751af
1 parent 6e22579
commit fc751af
Show file tree

Hide file tree

Showing 24 changed files with 89 additions and 567 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -17,12 +17,6 @@ repos:
       - id: trailing-whitespace
         args: [--markdown-linebreak-ext=md]
 
-  - repo: https://github.com/igorshubovych/markdownlint-cli
-    rev: v0.33.0
-    hooks:
-      - id: markdownlint
-        args: [-c, .markdownlint.yaml, --fix]
-
   - repo: https://github.com/pre-commit/mirrors-prettier
     rev: v3.0.0-alpha.6
     hooks:

diff --git a/...LA_Autoware/carla_autoware/CMakeLists.txt → ...LA_Autoware/carla_autoware/CMakeLists.txt b/...LA_Autoware/carla_autoware/CMakeLists.txt → ...LA_Autoware/carla_autoware/CMakeLists.txt
diff --git a/...r/CARLA_Autoware/carla_autoware/README.md → ...e/CARLA_Autoware/carla_autoware/README.md b/...r/CARLA_Autoware/carla_autoware/README.md → ...e/CARLA_Autoware/carla_autoware/README.md
@@ -7,8 +7,7 @@
 This ros package enables autonomous driving using Autoware in addition to the basic function of the official [ros-bridge](https://github.com/carla-simulator/ros-bridge) package (communication between ros and carla). (<https://github.com/gezp> for ROS2 Humble)
 
 - Make sure to Download the Python egg for 3.10 from [here](https://github.com/gezp/carla_ros/releases/tag/carla-0.9.15-ubuntu-22.04).
-- Add the egg file to the folder: ../CARLA_0.9.15/PythonAPI/carla/dist
-- And install the wheel using pip.
+- Install .whl using pip.
 
 # Environment
 
@@ -22,15 +21,16 @@ This ros package enables autonomous driving using Autoware in addition to the ba
 
 - [Autoware.Universe](https://autowarefoundation.github.io/autoware-documentation/galactic/installation/autoware/source-installation/)
 - [CARLA Installation](https://carla.readthedocs.io/en/latest/start_quickstart/)
-- [autoware containts](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/)
+- [Carla Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/)
+- [Carla Sensor Kit](https://github.com/mraditya01/carla_sensor_kit_launch)
+- [Autoware Individual params (forked with CARLA Sensor Kit params)](https://github.com/mraditya01/autoware_individual_params)
   1. Download maps (y-axis inverted version) to arbitaly location
   2. Change names. (point_cloud/Town01.pcd -> Town01/pointcloud_map.pcd, vector_maps/lanelet2/Town01.osm -> Town01/lanelet2_map.osm)
   3. Create `map_projector_info.yaml` and add `projector_type: local` on the first line.
-- Clone this repositories for ros-bridge on ROS2 Humble.
+- Clone this repositories for CARLA ros-bridge on ROS2 Humble.
 
   ```
-  git clone --recurse-submodules https://github.com/gezp/carla_ros.git -b humble-carla-0.9.14
-  git clone https://github.com/astuff/astuff_sensor_msgs.git
+  git clone --recurse-submodules https://github.com/mraditya01/carla_ros_bridge.git
   ```
 
 ## build
@@ -52,7 +52,7 @@ cd CARLA
 2. Run ros nodes
 
 ```bash
-ros2 launch autoware_launch e2e_simulator.launch.xml map_path:=$HOME/autoware_map/carla_town_01 vehicle_model:=sample_vehicle sensor_model:=carla_sensor_kit simulator_type:=carla
+ros2 launch carla_autoware e2e_simulator.launch.xml map_path:=$HOME/autoware_map/carla_town_01 vehicle_model:=sample_vehicle sensor_model:=carla_sensor_kit simulator_type:=carla
 ```
 
 3. Set initial pose (Init by GNSS)
@@ -65,3 +65,4 @@ ros2 launch autoware_launch e2e_simulator.launch.xml map_path:=$HOME/autoware_ma
 - If you want to edit the sensors configuration used in CARLA, edit `objects.json` located in `carla_autoware/config`.
 - You will also need to edit the `carla_sensor_kit_description` if you change the sensor configuration.
 - Misalignment might occurs during initialization, pressing `init by gnss` button should fix it.
+- Simulation time somtimes slowed down to 0.9x realtime.
diff --git a/...toware/carla_autoware/config/objects.json → ...toware/carla_autoware/config/objects.json b/...toware/carla_autoware/config/objects.json → ...toware/carla_autoware/config/objects.json
@@ -24,33 +24,19 @@
           "type": "sensor.camera.rgb",
           "id": "rgb_front",
           "spawn_point": { "x": 0.7, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 0.0, "yaw": 0.0 },
-          "image_size_x": 1280,
-          "image_size_y": 720,
-          "fov": 100.0
-        },
-        {
-          "type": "sensor.camera.rgb",
-          "id": "rgb_view",
-          "spawn_point": { "x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0 },
-          "image_size_x": 800,
-          "image_size_y": 600,
-          "fov": 90.0,
-          "attached_objects": [
-            {
-              "type": "actor.pseudo.control",
-              "id": "control"
-            }
-          ]
+          "image_size_x": 480,
+          "image_size_y": 360,
+          "fov": 90.0
         },
         {
           "type": "sensor.lidar.ray_cast",
           "id": "lidar",
           "spawn_point": { "x": 0.0, "y": 0.0, "z": 2.6, "roll": 0.0, "pitch": 0.0, "yaw": 0.0 },
           "range": 50,
-          "channels": 32,
-          "points_per_second": 320000,
-          "upper_fov": 2.0,
-          "lower_fov": -26.8,
+          "channels": 64,
+          "points_per_second": 480000,
+          "upper_fov": 3.0,
+          "lower_fov": -27.0,
           "rotation_frequency": 20,
           "noise_stddev": 0.0
         },
@@ -84,11 +70,6 @@
           "id": "collision",
           "spawn_point": { "x": 0.0, "y": 0.0, "z": 0.0 }
         },
-        {
-          "type": "sensor.other.lane_invasion",
-          "id": "lane_invasion",
-          "spawn_point": { "x": 0.0, "y": 0.0, "z": 0.0 }
-        },
         {
           "type": "sensor.pseudo.tf",
           "id": "tf"
@@ -101,10 +82,6 @@
           "type": "sensor.pseudo.odom",
           "id": "odometry"
         },
-        {
-          "type": "sensor.pseudo.speedometer",
-          "id": "speedometer"
-        },
         {
           "type": "actor.pseudo.control",
           "id": "control"

diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml
@@ -0,0 +1,6 @@
+<launch>
+  <group>
+    <node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="ego_vehicle2base_link" args="0 0 0 0 0 0 /ego_vehicle /base_link "/>
+  </group>
+</launch>
diff --git a/...arla_autoware/launch/carla_ros.launch.xml → ...arla_autoware/launch/carla_ros.launch.xml b/...arla_autoware/launch/carla_ros.launch.xml → ...arla_autoware/launch/carla_ros.launch.xml
@@ -9,19 +9,20 @@
   <arg name="passive" default="False"/>
   <arg name="synchronous_mode" default="True"/>
   <arg name="synchronous_mode_wait_for_vehicle_control_command" default="False"/>
-  <arg name="fixed_delta_seconds" default="0.05"/>
+  <arg name="fixed_delta_seconds" default="0.1"/>
   <arg name="role_name" default="ego_vehicle"/>
   <arg name="spawn_point_ego_vehicle" default="None"/>
   <arg name="spawn_sensors_only" default="False"/>
   <arg name="control_id" default="control"/>
   <arg name="register_all_sensors" default="True"/>
   <arg name="ego_vehicle_role_name" default="['hero', 'ego_vehicle', 'hero0', 'hero1', 'hero2', 'hero3', 'hero4', 'hero5', 'hero6', 'hero7', 'hero8', 'hero9']"/>
 
+  <!-- CARLA Object Definition Location -->
   <arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
 
   <node pkg="carla_ros_bridge" exec="bridge" name="carla_ros_bridge" output="screen">
     <!-- Parameters -->
-    <param name="use_sim_time" value="true"/>
+    <param name="use_sim_time" value="True"/>
     <param name="host" value="$(var host)"/>
     <param name="port" value="$(var port)"/>
     <param name="timeout" value="$(var timeout)"/>
@@ -32,6 +33,11 @@
     <param name="town" value="$(var town)"/>
     <param name="register_all_sensors" value="$(var register_all_sensors)"/>
     <param name="ego_vehicle_role_name" value="$(var ego_vehicle_role_name)"/>
+    <!-- Remap -->
+    <remap from="/carla/ego_vehicle/lidar" to="/sensing/lidar/top/outlier_filtered/pointcloud"/>
+    <remap from="/carla/ego_vehicle/imu" to="/sensing/imu/tamagawa/imu_raw"/>
+    <remap from="/carla/ego_vehicle/rgb_front/image" to="/sensing/camera/traffic_light/image_raw"/>
+    <remap from="/carla/ego_vehicle/rgb_front/camera_info" to="/sensing/camera/traffic_light/camera_info"/>
   </node>
 
   <node pkg="carla_spawn_objects" exec="carla_spawn_objects" name="carla_spawn_objects" output="screen">
@@ -40,10 +46,4 @@
     <param name="spawn_point_ego_vehicle" value="$(var spawn_point_ego_vehicle)"/>
     <param name="spawn_sensors_only" value="$(var spawn_sensors_only)"/>
   </node>
-
-  <node pkg="carla_spawn_objects" exec="set_initial_pose" name="set_initial_pose" output="screen">
-    <!-- Parameters -->
-    <param name="role_name" value="$(var role_name)"/>
-    <param name="control_id" value="$(var control_id)"/>
-  </node>
 </launch>
diff --git a/...CARLA_Autoware/carla_autoware/package.xml → ...CARLA_Autoware/carla_autoware/package.xml b/...CARLA_Autoware/carla_autoware/package.xml → ...CARLA_Autoware/carla_autoware/package.xml
@@ -13,6 +13,7 @@
   <depend>astuff_sensor_msgs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>carla_data_provider</depend>
   <depend>carla_msgs</depend>
   <depend>datetime</depend>
   <depend>geometry_msgs</depend>

diff --git a/...re/carla_autoware/resource/carla_autoware → ...re/carla_autoware/resource/carla_autoware b/...re/carla_autoware/resource/carla_autoware → ...re/carla_autoware/resource/carla_autoware
diff --git a/...r/CARLA_Autoware/carla_autoware/setup.cfg → ...e/CARLA_Autoware/carla_autoware/setup.cfg b/...r/CARLA_Autoware/carla_autoware/setup.cfg → ...e/CARLA_Autoware/carla_autoware/setup.cfg
diff --git a/...or/CARLA_Autoware/carla_autoware/setup.py → ...re/CARLA_Autoware/carla_autoware/setup.py b/...or/CARLA_Autoware/carla_autoware/setup.py → ...re/CARLA_Autoware/carla_autoware/setup.py
diff --git a/...ware/src/carla_autoware/carla_autoware.py → ...ware/src/carla_autoware/carla_autoware.py b/...ware/src/carla_autoware/carla_autoware.py → ...ware/src/carla_autoware/carla_autoware.py
@@ -3,28 +3,21 @@
 from autoware_auto_vehicle_msgs.msg import GearReport
 from autoware_auto_vehicle_msgs.msg import SteeringReport
 from autoware_auto_vehicle_msgs.msg import VelocityReport
-import carla
+from autoware_perception_msgs.msg import TrafficSignalArray
 from carla_msgs.msg import CarlaEgoVehicleControl
 from carla_msgs.msg import CarlaEgoVehicleStatus
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
 import rclpy
 from rclpy.node import Node
-from rclpy.qos import QoSProfile
-from sensor_msgs.msg import Imu
-from sensor_msgs.msg import PointCloud2
 
 
 class CarlaVehicleInterface(Node):
     def __init__(self):
         super().__init__("carla_vehicle_interface_node")
-
-        client = carla.Client("localhost", 2000)
-        client.set_timeout(30)
-
-        self._world = client.get_world()
         self.current_vel = 0.0
         self.target_vel = 0.0
         self.vel_diff = 0.0
-        self.current_control = carla.VehicleControl()
 
         # Publishes Topics used for AUTOWARE
         self.pub_vel_state = self.create_publisher(
@@ -40,33 +33,76 @@ def __init__(self):
         self.pub_control = self.create_publisher(
             CarlaEgoVehicleControl, "/carla/ego_vehicle/vehicle_control_cmd", 1
         )
-        self.vehicle_imu_publisher = self.create_publisher(Imu, "/sensing/imu/tamagawa/imu_raw", 1)
-        self.sensing_cloud_publisher = self.create_publisher(PointCloud2, "/carla_pointcloud", 1)
+        self.pub_traffic_signal_info = self.create_publisher(
+            TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1
+        )
+        self.pub_pose_with_cov = self.create_publisher(
+            PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1
+        )
 
         # Subscribe Topics used in Control
         self.sub_status = self.create_subscription(
             CarlaEgoVehicleStatus, "/carla/ego_vehicle/vehicle_status", self.ego_status_callback, 1
         )
         self.sub_control = self.create_subscription(
-            AckermannControlCommand,
-            "/control/command/control_cmd",
-            self.control_callback,
-            qos_profile=QoSProfile(depth=1),
+            AckermannControlCommand, "/control/command/control_cmd", self.control_callback, 1
         )
-        self.sub_imu = self.create_subscription(Imu, "/carla/ego_vehicle/imu", self.publish_imu, 1)
-        self.sub_lidar = self.create_subscription(
-            PointCloud2,
-            "/carla/ego_vehicle/lidar",
-            self.publish_lidar,
-            qos_profile=QoSProfile(depth=1),
+        self.sub_odom = self.create_subscription(
+            Odometry, "/carla/ego_vehicle/odometry", self.pose_callback, 1
         )
 
+    def pose_callback(self, pose_msg):
+        out_pose_with_cov = PoseWithCovarianceStamped()
+        out_pose_with_cov.header.frame_id = "map"
+        out_pose_with_cov.header.stamp = pose_msg.header.stamp
+        out_pose_with_cov.pose.pose = pose_msg.pose.pose
+        out_pose_with_cov.pose.covariance = [
+            0.1,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.1,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.1,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]
+        self.pub_pose_with_cov.publish(out_pose_with_cov)
+
     def ego_status_callback(self, in_status):
         """Convert and publish CARLA Ego Vehicle Status to AUTOWARE."""
         out_vel_state = VelocityReport()
         out_steering_state = SteeringReport()
         out_ctrl_mode = ControlModeReport()
         out_gear_state = GearReport()
+        out_traffic = TrafficSignalArray()
 
         out_vel_state.header = in_status.header
         out_vel_state.header.frame_id = "base_link"
@@ -88,6 +124,7 @@ def ego_status_callback(self, in_status):
         self.pub_steering_state.publish(out_steering_state)
         self.pub_ctrl_mode.publish(out_ctrl_mode)
         self.pub_gear_state.publish(out_gear_state)
+        self.pub_traffic_signal_info.publish(out_traffic)
 
     def control_callback(self, in_cmd):
         """Convert and publish CARLA Ego Vehicle Control to AUTOWARE."""
@@ -110,16 +147,6 @@ def control_callback(self, in_cmd):
         out_cmd.steer = -in_cmd.lateral.steering_tire_angle
         self.pub_control.publish(out_cmd)
 
-    def publish_lidar(self, lidar_msg):
-        """Publish LIDAR to Interface."""
-        lidar_msg.header.frame_id = "velodyne_top"
-        self.sensing_cloud_publisher.publish(lidar_msg)
-
-    def publish_imu(self, imu_msg):
-        """Publish IMU to Autoware."""
-        imu_msg.header.frame_id = "tamagawa/imu_link"
-        self.vehicle_imu_publisher.publish(imu_msg)
-
 
 def main(args=None):
     rclpy.init()

diff --git a/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml
diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt
diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/README.md b/simulator/CARLA_Autoware/carla_gnss_interface/README.md