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/carla_autoware/CMakeLists.txt b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/CMakeLists.txt similarity index 100% rename from simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/CMakeLists.txt diff --git a/simulator/CARLA_Autoware/carla_autoware/README.md b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/README.md similarity index 73% rename from simulator/CARLA_Autoware/carla_autoware/README.md rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/README.md index e77a9f920f6e2..8e5abd137c67c 100644 --- a/simulator/CARLA_Autoware/carla_autoware/README.md +++ b/simulator/CARLA_Autoware/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). ( 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/simulator/CARLA_Autoware/carla_autoware/config/objects.json b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/config/objects.json similarity index 71% rename from simulator/CARLA_Autoware/carla_autoware/config/objects.json rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/config/objects.json index 914a373af56d1..9e5d19731be03 100644 --- a/simulator/CARLA_Autoware/carla_autoware/config/objects.json +++ b/simulator/CARLA_Autoware/CARLA_Autoware/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 new file mode 100644 index 0000000000000..fc115f9ceaa53 --- /dev/null +++ b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml similarity index 79% rename from simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml index 1bed1146b4820..1b8b9870f1ddf 100644 --- a/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml +++ b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml @@ -9,7 +9,7 @@ - + @@ -17,11 +17,12 @@ + - + @@ -32,6 +33,11 @@ + + + + + @@ -40,10 +46,4 @@ - - - - - - diff --git a/simulator/CARLA_Autoware/carla_autoware/package.xml b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/package.xml similarity index 96% rename from simulator/CARLA_Autoware/carla_autoware/package.xml rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/package.xml index 859139d93b1ce..de94dd7499ea2 100644 --- a/simulator/CARLA_Autoware/carla_autoware/package.xml +++ b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/package.xml @@ -13,6 +13,7 @@ astuff_sensor_msgs autoware_auto_control_msgs autoware_auto_vehicle_msgs + carla_data_provider carla_msgs datetime geometry_msgs diff --git a/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/resource/carla_autoware similarity index 100% rename from simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/resource/carla_autoware diff --git a/simulator/CARLA_Autoware/carla_autoware/setup.cfg b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.cfg similarity index 100% rename from simulator/CARLA_Autoware/carla_autoware/setup.cfg rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.cfg diff --git a/simulator/CARLA_Autoware/carla_autoware/setup.py b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.py similarity index 100% rename from simulator/CARLA_Autoware/carla_autoware/setup.py rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.py diff --git a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py similarity index 67% rename from simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py rename to simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py index 425dd4ea66221..92fef61976efc 100644 --- a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/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 deleted file mode 100644 index 54bf9db5019d3..0000000000000 --- a/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt deleted file mode 100644 index 9715894c554ce..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(carla_gnss_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - - -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types ) -endif() - - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -find_package(PkgConfig REQUIRED) -pkg_check_modules(PROJ REQUIRED proj) - -ament_auto_add_library(carla_gnss_interface SHARED - src/carla_gnss_interface/carla_gnss_interface_node.cpp -) - -target_link_libraries(carla_gnss_interface ${PROJ_LIBRARIES}) - -rclcpp_components_register_node(carla_gnss_interface - PLUGIN "GnssInterface" - EXECUTABLE carla_gnss_interface_node -) - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/README.md b/simulator/CARLA_Autoware/carla_gnss_interface/README.md deleted file mode 100644 index 8e3ec20befe68..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/README.md +++ /dev/null @@ -1,7 +0,0 @@ -# gnss Interface Node - -Convert GNSS CARLA messages to pose and pose with covariance - -1. Receive GNSS Messages: Subscribe to the GNSS messages published by CARLA `carla/ego_vehicle/gnss`. These messages typically include latitude, longitude, and altitude. - -2. Convert to ROS Pose and Pose with Covariance: Extract the position components (latitude, longitude, altitude) from the GNSS messages and create a ROS geometry_msgs/Pose and geometry_msgs/PoseWithCovariance message. Set the position fields (x, y, z) of the ROS Pose message by converting the corresponding latitude, longitude, and altitude values using proj. diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp b/simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp deleted file mode 100644 index a3a237fd5935c..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2024 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CARLA_GNSS_INTERFACE__CARLA_GNSS_INTERFACE_NODE_HPP_ -#define CARLA_GNSS_INTERFACE__CARLA_GNSS_INTERFACE_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include - -#include -#include - -#include -#include -#include - -class GnssInterface : public rclcpp::Node -{ -public: - explicit GnssInterface(const rclcpp::NodeOptions & node_options); - std::string tf_output_frame_; - -private: - rclcpp::Subscription::SharedPtr sub_gnss_fix; - rclcpp::Publisher::SharedPtr pup_pose; - rclcpp::Publisher::SharedPtr pup_pose_with_cov; - - void GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr msg); -}; - -namespace interface -{ -namespace gnss -{ - -class GPSPoint -{ -public: - double x, y, z; - double lat, lon, alt; - double dir, a; - - GPSPoint() - { - x = y = z = 0; - lat = lon = alt = 0; - dir = a = 0; - } - - GPSPoint(const double & x, const double & y, const double & z, const double & a) - { - this->x = x; - this->y = y; - this->z = z; - this->a = a; - - lat = 0; - lon = 0; - alt = 0; - dir = 0; - } -}; - -class WayPoint -{ -public: - GPSPoint pos; - WayPoint() {} - - WayPoint(const double & x, const double & y, const double & z, const double & a) - { - pos.x = x; - pos.y = y; - pos.z = z; - pos.a = a; - } -}; - -struct MappingParameters -{ - std::string proj_str; - WayPoint org; - double lat; - double lon; - double alt; -}; - -void llaToxyz(const MappingParameters & params, double & x_out, double & y_out, double & z_out); - -} // namespace gnss -} // namespace interface - -#endif // CARLA_GNSS_INTERFACE__CARLA_GNSS_INTERFACE_NODE_HPP_ diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml b/simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml deleted file mode 100644 index 6faf90f770022..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/package.xml b/simulator/CARLA_Autoware/carla_gnss_interface/package.xml deleted file mode 100644 index 74aeac6e9c643..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - carla_gnss_interface - 1.0.0 - Convert GNSS CARLA messages to pose and pose with covariance - Muhammad Raditya Giovanni - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - - angles - diagnostic_updater - geometry_msgs - message_filters - proj - rclcpp - rclcpp_components - sensor_msgs - tf2 - tf2_ros - tinyxml - - - ament_cmake - - diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp b/simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp deleted file mode 100644 index a12da2b13b014..0000000000000 --- a/simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2024 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "carla_gnss_interface/carla_gnss_interface_node.hpp" - -#include - -namespace interface -{ -namespace gnss -{ - -void llaToxyz(const MappingParameters & params, double & x_out, double & y_out, double & z_out) -{ - if (params.proj_str.size() < 8) return; - - PJ_CONTEXT * C = proj_context_create(); - PJ * P = proj_create_crs_to_crs(C, "EPSG:4326", params.proj_str.c_str(), NULL); - - if (P == nullptr) { - proj_context_destroy(C); - return; - } - - PJ_COORD gps_degrees = proj_coord(params.lat, params.lon, params.alt, 0); - PJ_COORD xyz_out = proj_trans(P, PJ_FWD, gps_degrees); - x_out = xyz_out.enu.e + params.org.pos.x; - y_out = xyz_out.enu.n + params.org.pos.y; - z_out = xyz_out.enu.u + params.org.pos.z; - - proj_destroy(P); - proj_context_destroy(C); -} - -} // namespace gnss -} // namespace interface - -void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr msg) -{ - geometry_msgs::msg::PoseStamped pose_; - geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_; - interface::gnss::WayPoint origin, p; - interface::gnss::MappingParameters params; - - // Create MappingParameters struct to hold the parameters - params.proj_str = - "+proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs"; - params.lat = msg->latitude; - params.lon = msg->longitude; - params.alt = msg->altitude; - - // Call llaToxyz function - interface::gnss::llaToxyz(params, p.pos.x, p.pos.y, p.pos.z); - - pose_.header = msg->header; - pose_.header.frame_id = "map"; - pose_.pose.position.x = p.pos.x; - pose_.pose.position.y = p.pos.y; - pose_.pose.position.z = p.pos.z; - - pose_cov_.header = pose_.header; - pose_cov_.pose.pose = pose_.pose; - - pup_pose->publish(pose_); - pup_pose_with_cov->publish(pose_cov_); -} - -GnssInterface::GnssInterface(const rclcpp::NodeOptions & node_options) -: Node("gnss_interface_node", node_options), tf_output_frame_("base_link") -{ - sub_gnss_fix = this->create_subscription( - "carla/ego_vehicle/gnss", 1, - std::bind(&GnssInterface::GnssCallBack, this, std::placeholders::_1)); - - pup_pose = this->create_publisher("/sensing/gnss/pose", 1); - pup_pose_with_cov = this->create_publisher( - "/sensing/gnss/pose_with_covariance", 1); -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(GnssInterface) diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt deleted file mode 100644 index 66f98f9efabd5..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(carla_pointcloud_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types ) -endif() - - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - - -find_package(Boost COMPONENTS signals) -find_package(PCL REQUIRED COMPONENTS common) - - - -find_package(PkgConfig REQUIRED) - -include_directories( - ${PCL_COMMON_INCLUDE_DIRS} -) - -ament_auto_add_library(carla_pointcloud_interface SHARED - src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp -) - - -# workaround to allow deprecated header to build on both galactic and rolling -if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) - target_compile_definitions(carla_pointcloud_interface PRIVATE - USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER - ) -endif() - -rclcpp_components_register_node(carla_pointcloud_interface - PLUGIN "PointCloudInterface" - EXECUTABLE carla_pointcloud_interface_node -) - - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md b/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md deleted file mode 100644 index c47a171bfc415..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# carla_pointcloud_interface - -Relaying subscribed message from CARLA to Autoware related LIDAR topic message. - -Message Subscribed: -`carla_pointcloud` - -Message Published: -`/points_raw` -`/sensing/lidar/top/outlier_filtered/pointcloud` -`/sensing/lidar/concatenated/pointcloud` diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp b/simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp deleted file mode 100644 index cc6bc5950cf81..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2024 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CARLA_POINTCLOUD_INTERFACE__CARLA_POINTCLOUD_INTERFACE_NODE_HPP_ -#define CARLA_POINTCLOUD_INTERFACE__CARLA_POINTCLOUD_INTERFACE_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include - -#include -#include -#include - -#include -#include -#include - -class PointCloudInterface : public rclcpp::Node -{ -public: - explicit PointCloudInterface(const rclcpp::NodeOptions & node_options); - -private: - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - rclcpp::Publisher::SharedPtr velodyne_points_raw; - rclcpp::Subscription::SharedPtr carla_cloud_; - std::string tf_output; - void processScan(const sensor_msgs::msg::PointCloud2::SharedPtr scanMsg); - void setupTF(); -}; - -#endif // CARLA_POINTCLOUD_INTERFACE__CARLA_POINTCLOUD_INTERFACE_NODE_HPP_ diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml b/simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml deleted file mode 100644 index 2b53ad1b2a50b..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml b/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml deleted file mode 100644 index efca9e80b54f6..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - carla_pointcloud_interface - 1.0.0 - Convert CARLA 4D point cloud message to Autoware XYZIRADT cloud message - Muhammad Raditya Giovanni - Apache License 2.0 - - autoware_cmake - - libpcl-all-dev - pcl_ros - rclcpp - rclcpp_components - sensor_msgs - tf2 - tf2_ros - - - ament_cmake - - diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp b/simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp deleted file mode 100644 index 27b89d7054408..0000000000000 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2024 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "carla_pointcloud_interface/carla_pointcloud_interface_node.hpp" - -#include - -#include - -void PointCloudInterface::processScan(const sensor_msgs::msg::PointCloud2::SharedPtr scanMsg) -{ - { - sensor_msgs::msg::PointCloud2 transformed_cloud; - if (pcl_ros::transformPointCloud(tf_output, *scanMsg, transformed_cloud, *tf_buffer_)) { - transformed_cloud.header.stamp = scanMsg->header.stamp; - velodyne_points_raw->publish(transformed_cloud); - } - } -} - -void PointCloudInterface::setupTF() -{ - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); -} - -PointCloudInterface::PointCloudInterface(const rclcpp::NodeOptions & node_options) -: Node("carla_pointcloud_interface_node", node_options), tf_output("base_link") -{ - carla_cloud_ = this->create_subscription( - "carla_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&PointCloudInterface::processScan, this, std::placeholders::_1)); - { - setupTF(); - velodyne_points_raw = - this->create_publisher("/points_raw", rclcpp::SensorDataQoS()); - } -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudInterface)