diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a5ca7b6cf37fa..7223a734c02d9 100644 --- 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/simulator/carla_autoware/CMakeLists.txt b/simulator/carla_autoware/CMakeLists.txt new file mode 100644 index 0000000000000..f4471accbffd0 --- /dev/null +++ b/simulator/carla_autoware/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(carla_autoware) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +ament_export_dependencies(rclpy) + +# Install launch files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) + + +ament_auto_package( + launch + resource + src +) +ament_package() diff --git a/simulator/carla_autoware/README.md b/simulator/carla_autoware/README.md new file mode 100644 index 0000000000000..6691bc6517410 --- /dev/null +++ b/simulator/carla_autoware/README.md @@ -0,0 +1,68 @@ +# CARLA_Autoware + +# ROS2/Autoware.universe bridge for CARLA simulator + +### Thanks to for ROS2 Humble support for CARLA ROS bridge + +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). ( 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). +- Install .whl using pip. + +# Environment + +| ubuntu | ros | carla | autoware | +| :----: | :----: | :----: | :-------------: | +| 22.04 | humble | 0.9.15 | universe/master | + +# Setup + +## install + +- [Autoware.Universe](https://autowarefoundation.github.io/autoware-documentation/galactic/installation/autoware/source-installation/) +- [CARLA Installation](https://carla.readthedocs.io/en/latest/start_quickstart/) +- [Carla Lanelet2 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 CARLA ros-bridge on ROS2 Humble. + + ``` + git clone --recurse-submodules https://github.com/mraditya01/carla_ros_bridge.git + ``` + +## build + +```bash +cd colcon_ws +colcon build --symlink-install +``` + +# Run + +1. Run carla, change map, spawn object if you need + +```bash +cd CARLA +./CarlaUE4.sh -prefernvidia -quality-level=Low -RenderOffScreen +``` + +2. Run ros nodes + +```bash +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) +4. Set goal position +5. Wait for planning +6. Engage + +# Tips + +- 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/simulator/carla_autoware/config/objects.json b/simulator/carla_autoware/config/objects.json new file mode 100644 index 0000000000000..9e5d19731be03 --- /dev/null +++ b/simulator/carla_autoware/config/objects.json @@ -0,0 +1,92 @@ +{ + "objects": [ + { + "type": "sensor.pseudo.traffic_lights", + "id": "traffic_lights" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.actor_list", + "id": "actor_list" + }, + { + "type": "sensor.pseudo.markers", + "id": "markers" + }, + { + "type": "vehicle.toyota.prius", + "id": "ego_vehicle", + "sensors": [ + { + "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": 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": 64, + "points_per_second": 480000, + "upper_fov": 3.0, + "lower_fov": -27.0, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": { "x": 0.0, "y": 0.0, "z": 1.6 }, + "noise_alt_stddev": 0.0, + "noise_lat_stddev": 0.0, + "noise_lon_stddev": 0.0, + "noise_alt_bias": 0.0, + "noise_lat_bias": 0.0, + "noise_lon_bias": 0.0 + }, + { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": { "x": 0.0, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 0.0, "yaw": 0.0 }, + "noise_accel_stddev_x": 0.0, + "noise_accel_stddev_y": 0.0, + "noise_accel_stddev_z": 0.0, + "noise_gyro_stddev_x": 0.0, + "noise_gyro_stddev_y": 0.0, + "noise_gyro_stddev_z": 0.0, + "noise_gyro_bias_x": 0.0, + "noise_gyro_bias_y": 0.0, + "noise_gyro_bias_z": 0.0 + }, + { + "type": "sensor.other.collision", + "id": "collision", + "spawn_point": { "x": 0.0, "y": 0.0, "z": 0.0 } + }, + { + "type": "sensor.pseudo.tf", + "id": "tf" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.odom", + "id": "odometry" + }, + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + } + ] +} diff --git a/simulator/carla_autoware/launch/carla_autoware.launch.xml b/simulator/carla_autoware/launch/carla_autoware.launch.xml new file mode 100644 index 0000000000000..fc115f9ceaa53 --- /dev/null +++ b/simulator/carla_autoware/launch/carla_autoware.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/simulator/carla_autoware/launch/carla_ros.launch.xml b/simulator/carla_autoware/launch/carla_ros.launch.xml new file mode 100644 index 0000000000000..1b8b9870f1ddf --- /dev/null +++ b/simulator/carla_autoware/launch/carla_ros.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/carla_autoware/package.xml b/simulator/carla_autoware/package.xml new file mode 100644 index 0000000000000..de94dd7499ea2 --- /dev/null +++ b/simulator/carla_autoware/package.xml @@ -0,0 +1,37 @@ + + + carla_autoware + 0.0.0 + The carla autoware bridge package + Muhammad Raditya Giovanni + Maxime CLEMENT + Apache License 2.0 + + ros2launch + ros_compatibility + std_msgs + astuff_sensor_msgs + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + carla_data_provider + carla_msgs + datetime + geometry_msgs + math + nav_msgs + numpy + rclpy + sensor_msgs + sensor_msgs_py + tf2 + tf2_ros + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/simulator/carla_autoware/resource/carla_autoware b/simulator/carla_autoware/resource/carla_autoware new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/simulator/carla_autoware/setup.cfg b/simulator/carla_autoware/setup.cfg new file mode 100644 index 0000000000000..6ca2d4bdad9a1 --- /dev/null +++ b/simulator/carla_autoware/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/carla_autoware +[install] +install_scripts=$base/lib/carla_autoware diff --git a/simulator/carla_autoware/setup.py b/simulator/carla_autoware/setup.py new file mode 100644 index 0000000000000..82dfb6581d776 --- /dev/null +++ b/simulator/carla_autoware/setup.py @@ -0,0 +1,42 @@ +from glob import glob +import os + +ROS_VERSION = int(os.environ["ROS_VERSION"]) + +if ROS_VERSION == 1: + from distutils.core import setup + + from catkin_pkg.python_setup import generate_distutils_setup + + d = generate_distutils_setup(packages=["carla_autoware"], package_dir={"": "src"}) + + setup(**d) + +elif ROS_VERSION == 2: + from setuptools import setup + + package_name = "carla_autoware" + + setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, glob("config/objects.json")), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/carla_autoware.launch.xml")), + (os.path.join("share", package_name), glob("launch/carla_ros.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="mradityagio", + maintainer_email="mradityagio@gmail.com", + description="CARLA ROS2 bridge for AUTOWARE", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["carla_autoware = carla_autoware.carla_autoware:main"], + }, + package_dir={"": "src"}, + ) diff --git a/simulator/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py new file mode 100644 index 0000000000000..92fef61976efc --- /dev/null +++ b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py @@ -0,0 +1,158 @@ +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_vehicle_msgs.msg import ControlModeReport +from autoware_auto_vehicle_msgs.msg import GearReport +from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_auto_vehicle_msgs.msg import VelocityReport +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 + + +class CarlaVehicleInterface(Node): + def __init__(self): + super().__init__("carla_vehicle_interface_node") + self.current_vel = 0.0 + self.target_vel = 0.0 + self.vel_diff = 0.0 + + # Publishes Topics used for AUTOWARE + self.pub_vel_state = self.create_publisher( + VelocityReport, "/vehicle/status/velocity_status", 1 + ) + self.pub_steering_state = self.create_publisher( + SteeringReport, "/vehicle/status/steering_status", 1 + ) + self.pub_ctrl_mode = self.create_publisher( + ControlModeReport, "/vehicle/status/control_mode", 1 + ) + self.pub_gear_state = self.create_publisher(GearReport, "/vehicle/status/gear_status", 1) + self.pub_control = self.create_publisher( + CarlaEgoVehicleControl, "/carla/ego_vehicle/vehicle_control_cmd", 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, 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" + out_vel_state.longitudinal_velocity = in_status.velocity + out_vel_state.lateral_velocity = 0.0 + out_vel_state.heading_rate = 0.0 + self.current_vel = in_status.velocity + + out_steering_state.stamp = in_status.header.stamp + out_steering_state.steering_tire_angle = -in_status.control.steer + + out_gear_state.stamp = in_status.header.stamp + out_gear_state.report = GearReport.DRIVE + + out_ctrl_mode.stamp = in_status.header.stamp + out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS + + self.pub_vel_state.publish(out_vel_state) + 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.""" + out_cmd = CarlaEgoVehicleControl() + self.target_vel = abs(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 + self.pub_control.publish(out_cmd) + + +def main(args=None): + rclpy.init() + node = CarlaVehicleInterface() + rclpy.spin(node) + + +if __name__ == "__main__": + main()