From fefb5e0ec490bd6fc179081b81837b5ca0fd2407 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 15 Mar 2024 13:48:15 +0900 Subject: [PATCH 01/12] Added CARLA Bridge --- .../GNSS_Interface/CMakeLists.txt | 38 ++++ .../CARLA_Autoware/GNSS_Interface/README.md | 4 + .../gnss_interface/gnss_interface_node.hpp | 105 +++++++++ .../launch/gnss_interface.launch.xml | 14 ++ .../CARLA_Autoware/GNSS_Interface/package.xml | 29 +++ .../gnss_interface/gnss_interface_node.cpp | 84 +++++++ bridge/CARLA_Autoware/LICENSE | 21 ++ bridge/CARLA_Autoware/README.md | 58 +++++ .../carla_autoware/CMakeLists.txt | 52 +++++ .../carla_autoware/config/objects.json | 118 ++++++++++ .../launch/carla_autoware.launch.xml | 11 + .../launch/carla_ros.launch.xml | 52 +++++ .../launch/e2e_simulator.launch.xml | 88 ++++++++ .../CARLA_Autoware/carla_autoware/package.xml | 46 ++++ .../carla_autoware/resource/carla_autoware | 0 .../CARLA_Autoware/carla_autoware/setup.cfg | 4 + bridge/CARLA_Autoware/carla_autoware/setup.py | 44 ++++ .../src/carla_autoware/carla_autoware.py | 134 +++++++++++ .../.markdown-link-check.json | 13 ++ .../.markdownlint.yaml | 9 + .../.pre-commit-config-optional.yaml | 6 + .../.pre-commit-config.yaml | 68 ++++++ .../carla_sensor_kit_launch/.prettierignore | 2 + .../carla_sensor_kit_launch/.prettierrc.yaml | 20 ++ .../carla_sensor_kit_launch/.yamllint.yaml | 23 ++ .../CODE_OF_CONDUCT.md | 132 +++++++++++ .../carla_sensor_kit_launch/CONTRIBUTING.md | 3 + .../carla_sensor_kit_launch/DISCLAIMER.md | 46 ++++ .../carla_sensor_kit_launch/LICENSE | 201 +++++++++++++++++ .../carla_sensor_kit_launch/NOTICE | 8 + .../carla_sensor_kit_launch/README.md | 3 + .../build_depends.repos | 9 + .../CMakeLists.txt | 11 + .../config/sensor_kit_calibration.yaml | 92 ++++++++ .../config/sensors_calibration.yaml | 15 ++ .../carla_sensor_kit_description/package.xml | 17 ++ .../urdf/sensor_kit.xacro | 209 ++++++++++++++++++ .../urdf/sensors.xacro | 30 +++ .../carla_sensor_kit_launch/CMakeLists.txt | 16 ++ .../sensor_kit.param.yaml | 0 .../data/traffic_light_camera.yaml | 45 ++++ .../launch/imu.launch.xml | 14 ++ .../launch/lidar.launch.xml | 43 ++++ .../launch/pointcloud_preprocessor.launch.py | 110 +++++++++ .../launch/sensing.launch.xml | 33 +++ .../carla_sensor_kit_launch/package.xml | 26 +++ .../common_carla_sensor_launch/CMakeLists.txt | 14 ++ .../launch/velodyne_lidar.launch.xml | 12 + .../launch/velodyne_node_container.launch.py | 208 +++++++++++++++++ .../common_carla_sensor_launch/package.xml | 22 ++ .../carla_sensor_kit_launch/setup.cfg | 15 ++ .../pointcloud_interface/CMakeLists.txt | 69 ++++++ .../pointcloud_interface/README.md | 4 + .../pointcloud_interface_node.hpp | 31 +++ .../launch/pointcloud_interface.xml | 3 + .../pointcloud_interface/package.xml | 35 +++ .../pointcloud_interface_node.cpp | 55 +++++ 57 files changed, 2574 insertions(+) create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/README.md create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/package.xml create mode 100644 bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp create mode 100644 bridge/CARLA_Autoware/LICENSE create mode 100644 bridge/CARLA_Autoware/README.md create mode 100644 bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/carla_autoware/config/objects.json create mode 100644 bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_autoware/package.xml create mode 100644 bridge/CARLA_Autoware/carla_autoware/resource/carla_autoware create mode 100644 bridge/CARLA_Autoware/carla_autoware/setup.cfg create mode 100644 bridge/CARLA_Autoware/carla_autoware/setup.py create mode 100644 bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml create mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/README.md create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/package.xml create mode 100644 bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp diff --git a/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt b/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt new file mode 100644 index 0000000000000..a817198813e60 --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(gnss_interface) +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(gnss_interface SHARED + src/gnss_interface/gnss_interface_node.cpp +) + +target_link_libraries(gnss_interface ${PROJ_LIBRARIES}) + +rclcpp_components_register_node(gnss_interface + PLUGIN "GnssInterface" + EXECUTABLE gnss_interface_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/README.md b/bridge/CARLA_Autoware/GNSS_Interface/README.md new file mode 100644 index 0000000000000..356de168b8d13 --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/README.md @@ -0,0 +1,4 @@ +# gnss Interface Node + +Convert GNSS CARLA messages to pose and pose with covariance + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp b/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp new file mode 100644 index 0000000000000..ec2c9f97b4caa --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp @@ -0,0 +1,105 @@ + +#ifndef GNSS_ +#define GNSS_ + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include + +class GnssInterface : public rclcpp::Node +{ +public: + explicit GnssInterface(const rclcpp::NodeOptions & node_options); + virtual ~GnssInterface(); + 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; + } + + std::string ToString() + { + std::stringstream str; + str.precision(12); + str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl; + str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl; + return str.str(); + } +}; + + +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; + } +}; + +class MappingUtils { +public: + MappingUtils(); + virtual ~MappingUtils(); + + static void llaToxyz(const std::string& proj_str, const WayPoint& origin, const double& lat, + const double& lon, const double& alt, double& x_out, double& y_out, double& z_out); +}; + +} +} + +#endif + + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml b/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml new file mode 100644 index 0000000000000..5a14dbc2ed04b --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/package.xml b/bridge/CARLA_Autoware/GNSS_Interface/package.xml new file mode 100644 index 0000000000000..0793b71f731bd --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/package.xml @@ -0,0 +1,29 @@ + + + + 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 + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_ros + tinyxml + proj + + + ament_cmake + + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp b/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp new file mode 100644 index 0000000000000..d4d1a29a31eae --- /dev/null +++ b/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp @@ -0,0 +1,84 @@ +#include "gnss_interface/gnss_interface_node.hpp" +#include + + +namespace interface { + namespace gnss { + +using namespace std; + + +MappingUtils::MappingUtils() { +} + +MappingUtils::~MappingUtils() { +} + + +void MappingUtils::llaToxyz(const std::string& proj_str, const WayPoint& origin, + const double& lat, const double& lon, const double& alt, + double& x_out, double& y_out, double& z_out) { + if (proj_str.size() < 8) return; + + PJ_CONTEXT *C = proj_context_create(); + PJ *P = proj_create_crs_to_crs(C, "EPSG:4326", proj_str.c_str(), NULL); + + if (P == 0) return; + + PJ_COORD gps_degrees = proj_coord(lat, lon, alt, 0); + PJ_COORD xyz_out = proj_trans(P, PJ_FWD, gps_degrees); + x_out = xyz_out.enu.e + origin.pos.x; + y_out = xyz_out.enu.n + origin.pos.y; + z_out = xyz_out.enu.u + origin.pos.z; + + proj_destroy(P); + proj_context_destroy(C); +} +} +} + + +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::MappingUtils::llaToxyz( + "+proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs", + origin, msg->latitude, msg->longitude, msg->altitude, + 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() +{ + +} + +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/bridge/CARLA_Autoware/LICENSE b/bridge/CARLA_Autoware/LICENSE new file mode 100644 index 0000000000000..b3c8c7e3fc138 --- /dev/null +++ b/bridge/CARLA_Autoware/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Giovanni Muhammad Raditya + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/bridge/CARLA_Autoware/README.md b/bridge/CARLA_Autoware/README.md new file mode 100644 index 0000000000000..4d616e4f6723f --- /dev/null +++ b/bridge/CARLA_Autoware/README.md @@ -0,0 +1,58 @@ +# CARLA_Autoware +# ROS2/Autoware.universe bridge for CARLA simulator + + +### Thanks to https://github.com/gezp 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). (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.14-ubuntu-22.04). +- Add the egg file to the folder: ../CARLA_0.9.14/PythonAPI/carla/dist +- And install the wheel using pip. + +# Environment +|ubuntu|ros|carla|autoware| +|:---:|:---:|:---:|:---:| +|22.04|humble|0.9.14|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/) +* [autoware containts](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) + 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 and ROSBridge + ``` + git clone https://github.com/mraditya01/CARLA_Autoware.git + git clone --recurse-submodules https://github.com/gezp/carla_ros.git -b humble-carla-0.9.14 + ``` + * Copy the files (sensor_kit_calibration.yaml, sensors.calibration.yaml) from folder "GNSS_interface/src/carla_sensor_kit_launch/carla_sensor_kit_description/config" to "src/param/autoware_individual_params/carla_sensor_kit". + + +## 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 +``` + +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 +``` + +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. \ No newline at end of file diff --git a/bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt b/bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt new file mode 100644 index 0000000000000..deb641375684e --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(carla_autoware) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + + +# find dependencies +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(sensor_msgs_py REQUIRED) +find_package(datetime REQUIRED) + +find_package(rclpy REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(carla_msgs REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(autoware_auto_control_msgs REQUIRED) +find_package(tier4_debug_msgs REQUIRED) +find_package(tier4_system_msgs REQUIRED) +find_package(autoware_adapi_v1_msgs REQUIRED) +find_package(transform3d REQUIRED) +find_package(math REQUIRED) +find_package(numpy REQUIRED) +find_package(carla_data_provider REQUIRED) + + + + +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + +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/bridge/CARLA_Autoware/carla_autoware/config/objects.json b/bridge/CARLA_Autoware/carla_autoware/config/objects.json new file mode 100644 index 0000000000000..ff458c22276db --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/config/objects.json @@ -0,0 +1,118 @@ +{ + "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": "sensor.pseudo.opendrive_map", + "id": "map" + }, + { + "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": 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" + } + ] + }, + { + "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, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.opendrive_map", + "id": "OpenDRIVE", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 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.other.lane_invasion", + "id": "lane_invasion", + "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": "sensor.pseudo.speedometer", + "id": "speedometer" + }, + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + } + ] +} diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml new file mode 100644 index 0000000000000..b84bdd889d53e --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml new file mode 100644 index 0000000000000..46bdd4bb86cd4 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml new file mode 100644 index 0000000000000..345054570bde7 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_autoware/package.xml b/bridge/CARLA_Autoware/carla_autoware/package.xml new file mode 100644 index 0000000000000..10b5b5b6101be --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/package.xml @@ -0,0 +1,46 @@ + + + carla_autoware + 0.0.0 + The carla_manual_control package + CARLA Simulator Team + MIT + + std_msgs + ros_compatibility + geometry_msgs + tf2 + tf2_ros + sensor_msgs + sensor_msgs_py + + nav_msgs + carla_msgs + autoware_auto_vehicle_msgs + autoware_auto_control_msgs + rclpy + numpy + datetime + carla_data_provider + astuff_sensor_msgs + + + math + transform3d + + + + + ros2launch + ament_cmake + + ament_lint_auto + ament_lint_common + + + + + + ament_python + + diff --git a/bridge/CARLA_Autoware/carla_autoware/resource/carla_autoware b/bridge/CARLA_Autoware/carla_autoware/resource/carla_autoware new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.cfg b/bridge/CARLA_Autoware/carla_autoware/setup.cfg new file mode 100644 index 0000000000000..5cb120ba598b4 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/carla_autoware +[install] +install_scripts=$base/lib/carla_autoware \ No newline at end of file diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.py b/bridge/CARLA_Autoware/carla_autoware/setup.py new file mode 100644 index 0000000000000..f0e39dcba107c --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/setup.py @@ -0,0 +1,44 @@ + + +""" +Setup for carla_manual_control +""" +import os +from glob import glob +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/*.json')), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.xml')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='mradityagio', + maintainer_email='mradityagio@gmail.com', + description='CARLA ROS2 bridge for AUTOWARE', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': ['carla_autoware = carla_autoware.carla_autoware:main'], + }, + package_dir={'': 'src'}, + ) diff --git a/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py new file mode 100644 index 0000000000000..471d0248318ef --- /dev/null +++ b/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py @@ -0,0 +1,134 @@ +import rclpy +from rclpy.node import Node +from autoware_auto_vehicle_msgs.msg import VelocityReport, SteeringReport, ControlModeReport, GearReport +from carla_msgs.msg import CarlaEgoVehicleStatus, CarlaEgoVehicleControl +from autoware_auto_control_msgs.msg import AckermannControlCommand +from sensor_msgs.msg import Imu, PointCloud2 +import carla +from rclpy.qos import QoSProfile +import math +from transforms3d.euler import quat2euler +from sensor_msgs_py.point_cloud2 import create_cloud +import threading + + +class CarlaVehicleInterface(Node): + def __init__(self): + rclpy.init(args=None) + + super().__init__('carla_vehicle_interface_node') + + client = carla.Client("localhost", 2000) + client.set_timeout(20) + + 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() + self.ros2_node = rclpy.create_node("carla_autoware") + + self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,)) + self.spin_thread.start() + + # Publishes Topics used for AUTOWARE + self.pub_vel_state = self.ros2_node.create_publisher(VelocityReport, '/vehicle/status/velocity_status', 1) + self.pub_steering_state = self.ros2_node.create_publisher(SteeringReport, '/vehicle/status/steering_status', 1) + self.pub_ctrl_mode = self.ros2_node.create_publisher(ControlModeReport, '/vehicle/status/control_mode', 1) + self.pub_gear_state = self.ros2_node.create_publisher(GearReport, '/vehicle/status/gear_status', 1) + self.pub_control = self.ros2_node.create_publisher(CarlaEgoVehicleControl, '/carla/ego_vehicle/vehicle_control_cmd', 1) + self.vehicle_imu_publisher = self.ros2_node.create_publisher(Imu, '/sensing/imu/tamagawa/imu_raw', 1) + self.sensing_cloud_publisher = self.ros2_node.create_publisher(PointCloud2, '/carla_pointcloud', 1) + + # Subscribe Topics used in Control + self.sub_status = self.ros2_node.create_subscription(CarlaEgoVehicleStatus, '/carla/ego_vehicle/vehicle_status', self.ego_status_callback, 1) + self.sub_control = self.ros2_node.create_subscription(AckermannControlCommand, '/control/command/control_cmd', self.control_callback, qos_profile=QoSProfile(depth=1)) + self.sub_imu = self.ros2_node.create_subscription(Imu, '/carla/ego_vehicle/imu', self.publish_imu, 1) + self.sub_lidar = self.ros2_node.create_subscription(PointCloud2, '/carla/ego_vehicle/lidar', self.publish_lidar, qos_profile=QoSProfile(depth=1)) + + + + + + + + def ego_status_callback(self, in_status): + """ + Callback function for CARLA Ego Vehicle Status to AUTOWARE + """ + out_vel_state = VelocityReport() + out_steering_state = SteeringReport() + out_ctrl_mode = ControlModeReport() + out_gear_state = GearReport() + + 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) + + def control_callback(self, in_cmd): + """ + Callback function for CARLA Control + """ + 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 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 publish_gnss(self, msg): + """ + Publish GNSS to Autoware + """ + self.publisher_map.publish(msg) + + +def main(args=None): + carla_vehicle_interface = CarlaVehicleInterface() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json new file mode 100644 index 0000000000000..dec3db1ad1c8d --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json @@ -0,0 +1,13 @@ +{ + "aliveStatusCodes": [200, 206, 403], + "ignorePatterns": [ + { + "pattern": "^http://localhost" + }, + { + "pattern": "^https://github.com/.*/discussions/new" + } + ], + "retryOn429": true, + "retryCount": 10 +} diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml new file mode 100644 index 0000000000000..df1f518dc0d48 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml @@ -0,0 +1,9 @@ +# See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules. +default: true +MD013: false +MD024: + siblings_only: true +MD033: false +MD041: false +MD046: false +MD049: false diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml new file mode 100644 index 0000000000000..a805f1201c414 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml @@ -0,0 +1,6 @@ +repos: + - repo: https://github.com/tcort/markdown-link-check + rev: v3.10.0 + hooks: + - id: markdown-link-check + args: [--config=.markdown-link-check.json] diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml new file mode 100644 index 0000000000000..46b8247bde927 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml @@ -0,0 +1,68 @@ +ci: + autofix_commit_msg: "ci(pre-commit): autofix" + autoupdate_commit_msg: "ci(pre-commit): autoupdate" + +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.3.0 + hooks: + - id: check-json + - id: check-merge-conflict + - id: check-toml + - id: check-xml + - id: check-yaml + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + args: [--markdown-linebreak-ext=md] + + - repo: https://github.com/igorshubovych/markdownlint-cli + rev: v0.31.1 + hooks: + - id: markdownlint + args: [-c, .markdownlint.yaml, --fix] + + - repo: https://github.com/pre-commit/mirrors-prettier + rev: v2.7.1 + hooks: + - id: prettier + + - repo: https://github.com/adrienverge/yamllint + rev: v1.26.3 + hooks: + - id: yamllint + + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.7.1 + hooks: + - id: flake8-ros + - id: prettier-xacro + - id: prettier-launch-xml + - id: prettier-package-xml + - id: ros-include-guard + - id: sort-package-xml + + - repo: https://github.com/shellcheck-py/shellcheck-py + rev: v0.8.0.4 + hooks: + - id: shellcheck + + - repo: https://github.com/scop/pre-commit-shfmt + rev: v3.5.1-1 + hooks: + - id: shfmt + args: [-w, -s, -i=4] + + - repo: https://github.com/pycqa/isort + rev: 5.10.1 + hooks: + - id: isort + + - repo: https://github.com/psf/black + rev: 22.6.0 + hooks: + - id: black + args: [--line-length=100] + +exclude: .svg diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore new file mode 100644 index 0000000000000..a3c34d00a1377 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore @@ -0,0 +1,2 @@ +*.param.yaml +*.rviz diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml new file mode 100644 index 0000000000000..e29bf32762769 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml @@ -0,0 +1,20 @@ +printWidth: 100 +tabWidth: 2 +overrides: + - files: package.xml + options: + printWidth: 1000 + xmlSelfClosingSpace: false + xmlWhitespaceSensitivity: ignore + + - files: "*.launch.xml" + options: + printWidth: 200 + xmlSelfClosingSpace: false + xmlWhitespaceSensitivity: ignore + + - files: "*.xacro" + options: + printWidth: 200 + xmlSelfClosingSpace: false + xmlWhitespaceSensitivity: ignore diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml new file mode 100644 index 0000000000000..6228c70f02da9 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml @@ -0,0 +1,23 @@ +extends: default + +ignore: | + .clang-tidy + *.param.yaml + +rules: + braces: + level: error + max-spaces-inside: 1 # To format with Prettier + comments: + level: error + min-spaces-from-content: 1 # To be compatible with C++ and Python + document-start: + level: error + present: false # Don't need document start markers + line-length: disable # Delegate to Prettier + truthy: + level: error + check-keys: false # To allow 'on' of GitHub Actions + quoted-strings: + level: error + required: only-when-needed # To keep consistent style diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md new file mode 100644 index 0000000000000..c493cad4da591 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md @@ -0,0 +1,132 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +- Demonstrating empathy and kindness toward other people +- Being respectful of differing opinions, viewpoints, and experiences +- Giving and gracefully accepting constructive feedback +- Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +- Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +- The use of sexualized language or imagery, and sexual attention or advances of + any kind +- Trolling, insulting or derogatory comments, and personal or political attacks +- Public or private harassment +- Publishing others' private information, such as a physical or email address, + without their explicit permission +- Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official e-mail address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +conduct@autoware.org. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][mozilla coc]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][faq]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[mozilla coc]: https://github.com/mozilla/diversity +[faq]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md new file mode 100644 index 0000000000000..22c7ee28e8d9a --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md @@ -0,0 +1,3 @@ +# Contributing + +See . diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md new file mode 100644 index 0000000000000..1b5a9bbe4d981 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md @@ -0,0 +1,46 @@ +DISCLAIMER + +“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0. +This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with +the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents +specified in this disclaimer below and will be deemed to consent to this +disclaimer without any objection upon utilizing or downloading Autoware. + +Disclaimer and Waiver of Warranties + +1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) + including but not limited to any representation or warranty (i) of fitness or + suitability for a particular purpose contemplated by the Users, (ii) of the + expected functions, commercial value, accuracy, or usefulness of the Service, + (iii) that the use by the Users of the Service complies with the laws and + regulations applicable to the Users or any internal rules established by + industrial organizations, (iv) that the Service will be free of interruption or + defects, (v) of the non-infringement of any third party's right and (vi) the + accuracy of the content of the Services and the software itself. + +2. The Autoware Foundation shall not be liable for any damage incurred by the + User that are attributable to the Autoware Foundation for any reasons + whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR + INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. + +3. A User shall be entirely responsible for the content posted by the User and + its use of any content of the Service or the Website. If the User is held + responsible in a civil action such as a claim for damages or even in a criminal + case, the Autoware Foundation and member companies, governments and academic & + non-profit organizations and their directors, officers, employees and agents + (collectively, the “Indemnified Parties”) shall be completely discharged from + any rights or assertions the User may have against the Indemnified Parties, or + from any legal action, litigation or similar procedures. + +Indemnity + +A User shall indemnify and hold the Indemnified Parties harmless from any of +their damages, losses, liabilities, costs or expenses (including attorneys' +fees or criminal compensation), or any claims or demands made against the +Indemnified Parties by any third party, due to or arising out of, or in +connection with utilizing Autoware (including the representations and +warranties), the violation of applicable Product Liability Law of each country +(including criminal case) or violation of any applicable laws by the Users, or +the content posted by the User or its use of any content of the Service or the +Website. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE b/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE new file mode 100644 index 0000000000000..261eeb9e9f8b2 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE b/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE new file mode 100644 index 0000000000000..60fbbe96174e5 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE @@ -0,0 +1,8 @@ +carla_sensor_kit_launch +Copyright 2021 The Autoware Foundation + +This product includes software developed at +The Autoware Foundation (https://www.autoware.org/). + +This product includes code developed by TIER IV. +Copyright 2020 TIER IV, Inc. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md new file mode 100644 index 0000000000000..856700225f228 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md @@ -0,0 +1,3 @@ +# carla_sensor_kit_launch + +A sensor kit configurations for CARLA Simulator diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos b/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos new file mode 100644 index 0000000000000..08e78b602a8d0 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos @@ -0,0 +1,9 @@ +repositories: + autoware_common: + type: git + url: https://github.com/autowarefoundation/autoware_common.git + version: main + sensor_component_description: + type: git + url: https://github.com/tier4/sensor_component_description.git + version: main diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt new file mode 100644 index 0000000000000..b2b320a645d80 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(carla_sensor_kit_description) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + urdf + config +) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml new file mode 100644 index 0000000000000..aa2aeb5b3a42e --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,92 @@ +sensor_kit_base_link: + camera0/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + camera1/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + camera2/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + camera3/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + camera4/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + camera5/camera_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + traffic_light_right_camera/camera_link: + x: -0.7 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + traffic_light_left_camera/camera_link: + x: 0.7 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + velodyne_top_base_link: + x: 0.0 + y: 0.0 + z: 1.0 + roll: 0.0 + pitch: 0.0 + yaw: 0 + velodyne_left_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + velodyne_right_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + gnss_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + tamagawa/imu_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml new file mode 100644 index 0000000000000..c25cc52357e97 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml @@ -0,0 +1,15 @@ +base_link: + sensor_kit_base_link: + x: 0.0 + y: 0.0 + z: 1.6 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + velodyne_rear_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml new file mode 100644 index 0000000000000..638e73fb15972 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml @@ -0,0 +1,17 @@ + + + + carla_sensor_kit_description + 0.1.0 + The carla_sensor_kit_description package + Hatem Darweesh + Apache License 2.0 + + ament_cmake_auto + + velodyne_description + + + ament_cmake + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro new file mode 100644 index 0000000000000..c40b383563186 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro @@ -0,0 +1,209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro new file mode 100644 index 0000000000000..cf7966f010927 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt new file mode 100644 index 0000000000000..0ba7432e6d898 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(carla_sensor_kit_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + data + config +) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml new file mode 100644 index 0000000000000..5525f8189b213 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml @@ -0,0 +1,45 @@ +image_width: 1920 +image_height: 1080 +camera_name: traffic_light/camera +camera_matrix: + rows: 3 + cols: 3 + data: + [ + 2410.755261, + 0.000000, + 922.621401, + 0.000000, + 2403.573140, + 534.752500, + 0.000000, + 0.000000, + 1.000000, + ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: + [ + 2370.254883, + 0.000000, + 920.136018, + 0.000000, + 0.000000, + 2388.885254, + 535.599668, + 0.000000, + 0.000000, + 0.000000, + 1.000000, + 0.000000, + ] diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml new file mode 100644 index 0000000000000..a5381436ebbc4 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml new file mode 100644 index 0000000000000..472051f25de01 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 0000000000000..e41aeef657456 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,110 @@ +# Copyright 2020 Tier IV, Inc. 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. + + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + + # set concat filter as a component + concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud"), + ], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/sensing/lidar/left/outlier_filtered/pointcloud", + "/sensing/lidar/right/outlier_filtered/pointcloud", + ], + "output_frame": LaunchConfiguration("base_frame"), + "timeout_sec": 0.01, + "input_twist_topic_type": "twist", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[], + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + target_container = ( + container + if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else LaunchConfiguration("container_name") + ) + + # load concat or passthrough filter + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=target_container, + condition=IfCondition(LaunchConfiguration("use_concat_filter")), + ) + + return [container, concat_loader] + + +def generate_launch_description(): + + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "False") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "pointcloud_preprocessor_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml new file mode 100644 index 0000000000000..02cc9df228c29 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml new file mode 100644 index 0000000000000..1d2e1dece2c20 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml @@ -0,0 +1,26 @@ + + + + carla_sensor_kit_launch + 0.1.0 + The carla_sensor_kit_launch package + Hatem Darweesh + Apache License 2.0 + + ament_cmake_auto + + common_sensor_launch + gnss_poser + pointcloud_preprocessor + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt new file mode 100644 index 0000000000000..76585c6207b3f --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) +project(common_carla_sensor_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml new file mode 100644 index 0000000000000..4dff1789703e6 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py new file mode 100644 index 0000000000000..0b27efb5d7099 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py @@ -0,0 +1,208 @@ +# Copyright 2020 Tier IV, Inc. 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. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def get_vehicle_mirror_info(context): + path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def launch_setup(context, *args, **kwargs): + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + nodes = [] + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + mirror_info = get_vehicle_mirror_info(context) + cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] + cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] + cropbox_parameters["min_z"] = mirror_info["min_height_offset"] + cropbox_parameters["max_z"] = mirror_info["max_height_offset"] + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + nodes.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "outlier_filtered/pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + # need unique name, otherwise all processes in same container and the node names then clash + name="velodyne_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=nodes, + ) + + distortion_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distortion_relay_component = ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="pointcloud_distortion_relay", + namespace="", + parameters=[ + {"input_topic": "mirror_cropped/pointcloud_ex"}, + {"output_topic": "rectified/pointcloud_ex"} + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # one way to add a ComposableNode conditional on a launch argument to a + # container. The `ComposableNode` itself doesn't accept a condition + distortion_loader = LoadComposableNodes( + composable_node_descriptions=[distortion_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("use_distortion_corrector")), + ) + distortion_relay_loader = LoadComposableNodes( + composable_node_descriptions=[distortion_relay_component], + target_container=container, + condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")), + ) + + return [container, distortion_loader, distortion_relay_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("base_frame", "base_link", "base frame id") + add_launch_arg("container_name", "velodyne_composable_node_container", "container name") + add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg( + "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" + ) + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml new file mode 100644 index 0000000000000..c8028c196dc83 --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml @@ -0,0 +1,22 @@ + + + + common_carla_sensor_launch + 0.1.0 + The common_carla_sensor_launch package + Hatem Darweesh + Apache License 2.0 + + ament_cmake_auto + + velodyne_driver + velodyne_monitor + velodyne_pointcloud + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg b/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg new file mode 100644 index 0000000000000..5214751c7ba6b --- /dev/null +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg @@ -0,0 +1,15 @@ +[flake8] +# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini +extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 +import-order-style = pep8 +max-line-length = 100 +show-source = true +statistics = true + +[isort] +profile=black +line_length=100 +force_sort_within_sections=true +force_single_line=true +reverse_relative=true +known_third_party=launch diff --git a/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt b/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt new file mode 100644 index 0000000000000..d83a30d877e08 --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(pointcloud_interface) +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) +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_path(YAML_CPP_INCLUDE_DIR + NAMES yaml_cpp.h + PATHS ${YAML_CPP_INCLUDE_DIRS}) +find_library(YAML_CPP_LIBRARY + NAMES YAML_CPP + PATHS ${YAML_CPP_LIBRARY_DIRS}) + +find_package(OpenCV REQUIRED) + +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + +include_directories( + ${OpenCV_INCLUDE_DIRS} + ${PCL_COMMON_INCLUDE_DIRS} +) + +ament_auto_add_library(pointcloud_interface SHARED + src/pointcloud_interface/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(pointcloud_interface PRIVATE + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + +rclcpp_components_register_node(pointcloud_interface + PLUGIN "PointCloudInterface" + EXECUTABLE pointcloud_interface_node +) + + +ament_auto_package( + INSTALL_TO_SHARE + launch +) + diff --git a/bridge/CARLA_Autoware/pointcloud_interface/README.md b/bridge/CARLA_Autoware/pointcloud_interface/README.md new file mode 100644 index 0000000000000..b292d1b6c1dcf --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/README.md @@ -0,0 +1,4 @@ +# carla_pointcloud_interface + +Convert Simple point cloud message to Autoware recognized Extended cloud message. + diff --git a/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp b/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp new file mode 100644 index 0000000000000..335a9ae4b46c3 --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp @@ -0,0 +1,31 @@ + +#ifndef MODEL_LOG_NODE_HPP_ +#define MODEL_LOG_NODE_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include + +class PointCloudInterface : public rclcpp::Node +{ +public: + explicit PointCloudInterface(const rclcpp::NodeOptions & node_options); + virtual ~PointCloudInterface(); + +private: + std::shared_ptr tf_buffer_; + rclcpp::Publisher::SharedPtr velodyne_points_raw; + rclcpp::Publisher::SharedPtr velodyne_points_top; + rclcpp::Publisher::SharedPtr velodyne_points_con; + std::shared_ptr tf_listener_; + rclcpp::Subscription::SharedPtr carla_cloud_; + std::string tf_output; + void processScan(const sensor_msgs::msg::PointCloud2::SharedPtr scanMsg); + void setupTF(); +}; + +#endif \ No newline at end of file diff --git a/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml b/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml new file mode 100644 index 0000000000000..8b6eb39bba2dd --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml @@ -0,0 +1,3 @@ + + + diff --git a/bridge/CARLA_Autoware/pointcloud_interface/package.xml b/bridge/CARLA_Autoware/pointcloud_interface/package.xml new file mode 100644 index 0000000000000..0aec53de9c4a4 --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/package.xml @@ -0,0 +1,35 @@ + + + + pointcloud_interface + 1.0.0 + Convert CARLA 4D point cloud message to Autoware XYZIRADT cloud message + Muhammad Raditya Giovanni + Apache License 2.0 + + ament_cmake_auto + + angles + diagnostic_updater + autoware_auto_vehicle_msgs + autoware_auto_control_msgs + geometry_msgs + libpcl-all-dev + message_filters + pcl_ros + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_ros + velodyne_msgs + visualization_msgs + yaml-cpp + cv_bridge + image_transport + + + ament_cmake + + diff --git a/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp b/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp new file mode 100644 index 0000000000000..b56b1424e555c --- /dev/null +++ b/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp @@ -0,0 +1,55 @@ +#include "pointcloud_interface/pointcloud_interface_node.hpp" +#include +#include +#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_top->publish(transformed_cloud); + velodyne_points_con->publish(transformed_cloud); + 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() +{ + +} + +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()); + velodyne_points_top = + this->create_publisher("/sensing/lidar/top/outlier_filtered/pointcloud", rclcpp::SensorDataQoS()); + velodyne_points_con = + this->create_publisher("/sensing/lidar/concatenated/pointcloud", rclcpp::SensorDataQoS()); + } +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudInterface) From 03381e048bf0acfffb17ae2f0f80c111509b377b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 15 Mar 2024 05:15:50 +0000 Subject: [PATCH 02/12] style(pre-commit): autofix --- .../GNSS_Interface/CMakeLists.txt | 3 +- .../CARLA_Autoware/GNSS_Interface/README.md | 1 - .../gnss_interface/gnss_interface_node.hpp | 141 ++++++------ .../launch/gnss_interface.launch.xml | 2 - .../CARLA_Autoware/GNSS_Interface/package.xml | 12 +- .../gnss_interface/gnss_interface_node.cpp | 103 ++++----- bridge/CARLA_Autoware/README.md | 43 ++-- .../carla_autoware/config/objects.json | 217 +++++++++--------- .../launch/carla_autoware.launch.xml | 1 - .../launch/carla_ros.launch.xml | 89 ++++--- .../launch/e2e_simulator.launch.xml | 10 +- .../CARLA_Autoware/carla_autoware/package.xml | 35 ++- .../CARLA_Autoware/carla_autoware/setup.cfg | 2 +- bridge/CARLA_Autoware/carla_autoware/setup.py | 38 +-- .../src/carla_autoware/carla_autoware.py | 118 ++++++---- .../carla_sensor_kit_launch/README.md | 2 +- .../launch/imu.launch.xml | 4 +- .../launch/pointcloud_preprocessor.launch.py | 2 - .../launch/velodyne_node_container.launch.py | 6 +- .../pointcloud_interface/CMakeLists.txt | 3 +- .../pointcloud_interface/README.md | 1 - .../pointcloud_interface_node.hpp | 21 +- .../pointcloud_interface/package.xml | 14 +- .../pointcloud_interface_node.cpp | 54 ++--- 24 files changed, 471 insertions(+), 451 deletions(-) diff --git a/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt b/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt index a817198813e60..a5129ffb3d2c7 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt +++ b/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt @@ -26,7 +26,7 @@ ament_auto_add_library(gnss_interface SHARED target_link_libraries(gnss_interface ${PROJ_LIBRARIES}) -rclcpp_components_register_node(gnss_interface +rclcpp_components_register_node(gnss_interface PLUGIN "GnssInterface" EXECUTABLE gnss_interface_node ) @@ -35,4 +35,3 @@ ament_auto_package( INSTALL_TO_SHARE launch ) - diff --git a/bridge/CARLA_Autoware/GNSS_Interface/README.md b/bridge/CARLA_Autoware/GNSS_Interface/README.md index 356de168b8d13..b3d5a12fa8142 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/README.md +++ b/bridge/CARLA_Autoware/GNSS_Interface/README.md @@ -1,4 +1,3 @@ # gnss Interface Node Convert GNSS CARLA messages to pose and pose with covariance - diff --git a/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp b/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp index ec2c9f97b4caa..034af17d42a12 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp +++ b/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp @@ -1,105 +1,104 @@ -#ifndef GNSS_ -#define GNSS_ +#ifndef GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ +#define GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ -#include -#include -#include -#include #include "rclcpp/rclcpp.hpp" -#include + #include #include +#include + #include +#include + +#include +#include +#include class GnssInterface : public rclcpp::Node { public: - explicit GnssInterface(const rclcpp::NodeOptions & node_options); - virtual ~GnssInterface(); - std::string tf_output_frame_; - -private: + explicit GnssInterface(const rclcpp::NodeOptions & node_options); + virtual ~GnssInterface(); + 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 { +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; - } - - std::string ToString() - { - std::stringstream str; - str.precision(12); - str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl; - str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl; - return str.str(); - } + 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; + } + + std::string ToString() + { + std::stringstream str; + str.precision(12); + str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl; + str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl; + return str.str(); + } }; - 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; - } + 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; + } }; -class MappingUtils { +class MappingUtils +{ public: - MappingUtils(); - virtual ~MappingUtils(); + MappingUtils(); + virtual ~MappingUtils(); - static void llaToxyz(const std::string& proj_str, const WayPoint& origin, const double& lat, - const double& lon, const double& alt, double& x_out, double& y_out, double& z_out); + static void llaToxyz( + const std::string & proj_str, const WayPoint & origin, const double & lat, const double & lon, + const double & alt, double & x_out, double & y_out, double & z_out); }; -} -} - -#endif - +} // namespace gnss +} // namespace interface +#endif // GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ diff --git a/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml b/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml index 5a14dbc2ed04b..556e94f2821b1 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml +++ b/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml @@ -9,6 +9,4 @@ - - diff --git a/bridge/CARLA_Autoware/GNSS_Interface/package.xml b/bridge/CARLA_Autoware/GNSS_Interface/package.xml index 0793b71f731bd..396359de45f38 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/package.xml +++ b/bridge/CARLA_Autoware/GNSS_Interface/package.xml @@ -12,17 +12,17 @@ autoware_cmake angles - diagnostic_updater - geometry_msgs - message_filters + diagnostic_updater + geometry_msgs + message_filters + proj rclcpp rclcpp_components sensor_msgs tf2 - tf2_ros + tf2_ros tinyxml - proj - + ament_cmake diff --git a/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp b/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp index d4d1a29a31eae..9b5543d458796 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp +++ b/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp @@ -1,84 +1,81 @@ #include "gnss_interface/gnss_interface_node.hpp" -#include +#include -namespace interface { - namespace gnss { +namespace interface +{ +namespace gnss +{ using namespace std; - -MappingUtils::MappingUtils() { +MappingUtils::MappingUtils() +{ } -MappingUtils::~MappingUtils() { +MappingUtils::~MappingUtils() +{ } +void MappingUtils::llaToxyz( + const std::string & proj_str, const WayPoint & origin, const double & lat, const double & lon, + const double & alt, double & x_out, double & y_out, double & z_out) +{ + if (proj_str.size() < 8) return; -void MappingUtils::llaToxyz(const std::string& proj_str, const WayPoint& origin, - const double& lat, const double& lon, const double& alt, - double& x_out, double& y_out, double& z_out) { - if (proj_str.size() < 8) return; - - PJ_CONTEXT *C = proj_context_create(); - PJ *P = proj_create_crs_to_crs(C, "EPSG:4326", proj_str.c_str(), NULL); + PJ_CONTEXT * C = proj_context_create(); + PJ * P = proj_create_crs_to_crs(C, "EPSG:4326", proj_str.c_str(), NULL); - if (P == 0) return; + if (P == 0) return; - PJ_COORD gps_degrees = proj_coord(lat, lon, alt, 0); - PJ_COORD xyz_out = proj_trans(P, PJ_FWD, gps_degrees); - x_out = xyz_out.enu.e + origin.pos.x; - y_out = xyz_out.enu.n + origin.pos.y; - z_out = xyz_out.enu.u + origin.pos.z; + PJ_COORD gps_degrees = proj_coord(lat, lon, alt, 0); + PJ_COORD xyz_out = proj_trans(P, PJ_FWD, gps_degrees); + x_out = xyz_out.enu.e + origin.pos.x; + y_out = xyz_out.enu.n + origin.pos.y; + z_out = xyz_out.enu.u + origin.pos.z; - proj_destroy(P); - proj_context_destroy(C); + 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::MappingUtils::llaToxyz( - "+proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs", - origin, msg->latitude, msg->longitude, msg->altitude, - 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_); - + geometry_msgs::msg::PoseStamped pose_; + geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_; + interface::gnss::WayPoint origin, p; + interface::gnss::MappingUtils::llaToxyz( + "+proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs", origin, + msg->latitude, msg->longitude, msg->altitude, 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() { - } 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); -} + 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/bridge/CARLA_Autoware/README.md b/bridge/CARLA_Autoware/README.md index 4d616e4f6723f..cbf6f4e75f2b0 100644 --- a/bridge/CARLA_Autoware/README.md +++ b/bridge/CARLA_Autoware/README.md @@ -1,48 +1,58 @@ # 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) -### Thanks to https://github.com/gezp 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). (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.14-ubuntu-22.04). +- Make sure to Download the Python egg for 3.10 from [here](https://github.com/gezp/carla_ros/releases/tag/carla-0.9.14-ubuntu-22.04). - Add the egg file to the folder: ../CARLA_0.9.14/PythonAPI/carla/dist - And install the wheel using pip. -# Environment -|ubuntu|ros|carla|autoware| -|:---:|:---:|:---:|:---:| -|22.04|humble|0.9.14|universe/master| +# Environment + +| ubuntu | ros | carla | autoware | +| :----: | :----: | :----: | :-------------: | +| 22.04 | humble | 0.9.14 | 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/) -* [autoware containts](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) + +- [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/) 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 and ROSBridge + 3. Create `map_projector_info.yaml` and add `projector_type: local` on the first line. +- Clone this repositories and ROSBridge + ``` git clone https://github.com/mraditya01/CARLA_Autoware.git git clone --recurse-submodules https://github.com/gezp/carla_ros.git -b humble-carla-0.9.14 ``` - * Copy the files (sensor_kit_calibration.yaml, sensors.calibration.yaml) from folder "GNSS_interface/src/carla_sensor_kit_launch/carla_sensor_kit_description/config" to "src/param/autoware_individual_params/carla_sensor_kit". +- Copy the files (sensor_kit_calibration.yaml, sensors.calibration.yaml) from folder "GNSS_interface/src/carla_sensor_kit_launch/carla_sensor_kit_description/config" to "src/param/autoware_individual_params/carla_sensor_kit". ## 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 ``` 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 ``` @@ -53,6 +63,7 @@ ros2 launch carla_autoware e2e_simulator.launch.xml map_path:=$HOME/autoware_map 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. \ No newline at end of file + +- 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. diff --git a/bridge/CARLA_Autoware/carla_autoware/config/objects.json b/bridge/CARLA_Autoware/carla_autoware/config/objects.json index ff458c22276db..ac6b39666ec5f 100644 --- a/bridge/CARLA_Autoware/carla_autoware/config/objects.json +++ b/bridge/CARLA_Autoware/carla_autoware/config/objects.json @@ -1,118 +1,125 @@ -{ - "objects": - [ +{ + "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": "sensor.pseudo.opendrive_map", + "id": "map" + }, + { + "type": "vehicle.toyota.prius", + "id": "ego_vehicle", + "sensors": [ { - "type": "sensor.pseudo.traffic_lights", - "id": "traffic_lights" + "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.pseudo.objects", - "id": "objects" + "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" + } + ] }, { - "type": "sensor.pseudo.actor_list", - "id": "actor_list" + "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, + "rotation_frequency": 20, + "noise_stddev": 0.0 }, { - "type": "sensor.pseudo.markers", - "id": "markers" + "type": "sensor.opendrive_map", + "id": "OpenDRIVE", + "spawn_point": { "x": 0.0, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 0.0 } }, + { - "type": "sensor.pseudo.opendrive_map", - "id": "map" + "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": "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": 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" - } - ] - }, - { - "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, - "rotation_frequency": 20, - "noise_stddev": 0.0 - }, - { - "type": "sensor.opendrive_map", - "id": "OpenDRIVE", - "spawn_point": {"x": 0.0, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 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.other.lane_invasion", - "id": "lane_invasion", - "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": "sensor.pseudo.speedometer", - "id": "speedometer" - }, - { - "type": "actor.pseudo.control", - "id": "control" - } - ] + "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.other.lane_invasion", + "id": "lane_invasion", + "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": "sensor.pseudo.speedometer", + "id": "speedometer" + }, + { + "type": "actor.pseudo.control", + "id": "control" } - ] + ] + } + ] } diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml index b84bdd889d53e..6e82013a84531 100644 --- a/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml +++ b/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml @@ -8,4 +8,3 @@ - diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml index 46bdd4bb86cd4..a3b1b892b68fb 100644 --- a/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml +++ b/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml @@ -1,52 +1,49 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml b/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml index 345054570bde7..8226a45c29088 100644 --- a/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml +++ b/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml @@ -32,22 +32,18 @@ - + - - - - - + + - diff --git a/bridge/CARLA_Autoware/carla_autoware/package.xml b/bridge/CARLA_Autoware/carla_autoware/package.xml index 10b5b5b6101be..2d50f4742f2eb 100644 --- a/bridge/CARLA_Autoware/carla_autoware/package.xml +++ b/bridge/CARLA_Autoware/carla_autoware/package.xml @@ -6,40 +6,33 @@ CARLA Simulator Team MIT - std_msgs + ros2launch ros_compatibility - geometry_msgs - tf2 - tf2_ros - sensor_msgs - sensor_msgs_py - - nav_msgs - carla_msgs - autoware_auto_vehicle_msgs + std_msgs + astuff_sensor_msgs autoware_auto_control_msgs - rclpy - numpy - datetime + autoware_auto_vehicle_msgs carla_data_provider - astuff_sensor_msgs - - + carla_msgs + datetime + geometry_msgs math + nav_msgs + numpy + rclpy + sensor_msgs + sensor_msgs_py + tf2 + tf2_ros transform3d - - ros2launch ament_cmake ament_lint_auto ament_lint_common - - - ament_python diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.cfg b/bridge/CARLA_Autoware/carla_autoware/setup.cfg index 5cb120ba598b4..6ca2d4bdad9a1 100644 --- a/bridge/CARLA_Autoware/carla_autoware/setup.cfg +++ b/bridge/CARLA_Autoware/carla_autoware/setup.cfg @@ -1,4 +1,4 @@ [develop] script_dir=$base/lib/carla_autoware [install] -install_scripts=$base/lib/carla_autoware \ No newline at end of file +install_scripts=$base/lib/carla_autoware diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.py b/bridge/CARLA_Autoware/carla_autoware/setup.py index f0e39dcba107c..c02295fdd714b 100644 --- a/bridge/CARLA_Autoware/carla_autoware/setup.py +++ b/bridge/CARLA_Autoware/carla_autoware/setup.py @@ -1,44 +1,44 @@ - - """ Setup for carla_manual_control """ -import os from glob import glob -ROS_VERSION = int(os.environ['ROS_VERSION']) +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'}) + d = generate_distutils_setup(packages=["carla_autoware"], package_dir={"": "src"}) setup(**d) elif ROS_VERSION == 2: from setuptools import setup - package_name = 'carla_autoware' + package_name = "carla_autoware" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, glob('config/*.json')), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*.launch.xml')) + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, glob("config/*.json")), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/*.launch.xml")), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='mradityagio', - maintainer_email='mradityagio@gmail.com', - description='CARLA ROS2 bridge for AUTOWARE', - license='MIT', - tests_require=['pytest'], + maintainer="mradityagio", + maintainer_email="mradityagio@gmail.com", + description="CARLA ROS2 bridge for AUTOWARE", + license="MIT", + tests_require=["pytest"], entry_points={ - 'console_scripts': ['carla_autoware = carla_autoware.carla_autoware:main'], + "console_scripts": ["carla_autoware = carla_autoware.carla_autoware:main"], }, - package_dir={'': 'src'}, + package_dir={"": "src"}, ) diff --git a/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py index 471d0248318ef..41aab41397860 100644 --- a/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py @@ -1,56 +1,84 @@ -import rclpy -from rclpy.node import Node -from autoware_auto_vehicle_msgs.msg import VelocityReport, SteeringReport, ControlModeReport, GearReport -from carla_msgs.msg import CarlaEgoVehicleStatus, CarlaEgoVehicleControl +import math +import threading + from autoware_auto_control_msgs.msg import AckermannControlCommand -from sensor_msgs.msg import Imu, PointCloud2 +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 import carla +from carla_msgs.msg import CarlaEgoVehicleControl +from carla_msgs.msg import CarlaEgoVehicleStatus +import rclpy +from rclpy.node import Node from rclpy.qos import QoSProfile -import math -from transforms3d.euler import quat2euler +from sensor_msgs.msg import Imu +from sensor_msgs.msg import PointCloud2 from sensor_msgs_py.point_cloud2 import create_cloud -import threading +from transforms3d.euler import quat2euler class CarlaVehicleInterface(Node): def __init__(self): rclpy.init(args=None) - super().__init__('carla_vehicle_interface_node') - + super().__init__("carla_vehicle_interface_node") + client = carla.Client("localhost", 2000) - client.set_timeout(20) - - self._world = client.get_world() + client.set_timeout(20) + + 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() self.ros2_node = rclpy.create_node("carla_autoware") - + self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,)) self.spin_thread.start() - + # Publishes Topics used for AUTOWARE - self.pub_vel_state = self.ros2_node.create_publisher(VelocityReport, '/vehicle/status/velocity_status', 1) - self.pub_steering_state = self.ros2_node.create_publisher(SteeringReport, '/vehicle/status/steering_status', 1) - self.pub_ctrl_mode = self.ros2_node.create_publisher(ControlModeReport, '/vehicle/status/control_mode', 1) - self.pub_gear_state = self.ros2_node.create_publisher(GearReport, '/vehicle/status/gear_status', 1) - self.pub_control = self.ros2_node.create_publisher(CarlaEgoVehicleControl, '/carla/ego_vehicle/vehicle_control_cmd', 1) - self.vehicle_imu_publisher = self.ros2_node.create_publisher(Imu, '/sensing/imu/tamagawa/imu_raw', 1) - self.sensing_cloud_publisher = self.ros2_node.create_publisher(PointCloud2, '/carla_pointcloud', 1) + self.pub_vel_state = self.ros2_node.create_publisher( + VelocityReport, "/vehicle/status/velocity_status", 1 + ) + self.pub_steering_state = self.ros2_node.create_publisher( + SteeringReport, "/vehicle/status/steering_status", 1 + ) + self.pub_ctrl_mode = self.ros2_node.create_publisher( + ControlModeReport, "/vehicle/status/control_mode", 1 + ) + self.pub_gear_state = self.ros2_node.create_publisher( + GearReport, "/vehicle/status/gear_status", 1 + ) + self.pub_control = self.ros2_node.create_publisher( + CarlaEgoVehicleControl, "/carla/ego_vehicle/vehicle_control_cmd", 1 + ) + self.vehicle_imu_publisher = self.ros2_node.create_publisher( + Imu, "/sensing/imu/tamagawa/imu_raw", 1 + ) + self.sensing_cloud_publisher = self.ros2_node.create_publisher( + PointCloud2, "/carla_pointcloud", 1 + ) # Subscribe Topics used in Control - self.sub_status = self.ros2_node.create_subscription(CarlaEgoVehicleStatus, '/carla/ego_vehicle/vehicle_status', self.ego_status_callback, 1) - self.sub_control = self.ros2_node.create_subscription(AckermannControlCommand, '/control/command/control_cmd', self.control_callback, qos_profile=QoSProfile(depth=1)) - self.sub_imu = self.ros2_node.create_subscription(Imu, '/carla/ego_vehicle/imu', self.publish_imu, 1) - self.sub_lidar = self.ros2_node.create_subscription(PointCloud2, '/carla/ego_vehicle/lidar', self.publish_lidar, qos_profile=QoSProfile(depth=1)) - - - - - - + self.sub_status = self.ros2_node.create_subscription( + CarlaEgoVehicleStatus, "/carla/ego_vehicle/vehicle_status", self.ego_status_callback, 1 + ) + self.sub_control = self.ros2_node.create_subscription( + AckermannControlCommand, + "/control/command/control_cmd", + self.control_callback, + qos_profile=QoSProfile(depth=1), + ) + self.sub_imu = self.ros2_node.create_subscription( + Imu, "/carla/ego_vehicle/imu", self.publish_imu, 1 + ) + self.sub_lidar = self.ros2_node.create_subscription( + PointCloud2, + "/carla/ego_vehicle/lidar", + self.publish_lidar, + qos_profile=QoSProfile(depth=1), + ) def ego_status_callback(self, in_status): """ @@ -62,18 +90,17 @@ def ego_status_callback(self, in_status): out_gear_state = GearReport() out_vel_state.header = in_status.header - out_vel_state.header.frame_id = 'base_link' + 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_steering_state.steering_tire_angle = -in_status.control.steer out_gear_state.stamp = in_status.header.stamp - out_gear_state.report = GearReport.DRIVE + out_gear_state.report = GearReport.DRIVE out_ctrl_mode.stamp = in_status.header.stamp out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS @@ -82,7 +109,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) - + def control_callback(self, in_cmd): """ Callback function for CARLA Control @@ -103,7 +130,7 @@ def control_callback(self, in_cmd): else: out_cmd.brake = 0.01 - out_cmd.steer = (-in_cmd.lateral.steering_tire_angle) + out_cmd.steer = -in_cmd.lateral.steering_tire_angle self.pub_control.publish(out_cmd) def publish_lidar(self, lidar_msg): @@ -111,24 +138,25 @@ def publish_lidar(self, lidar_msg): Publish LIDAR to Interface """ lidar_msg.header.frame_id = "velodyne_top" - self.sensing_cloud_publisher.publish(lidar_msg) - + 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 publish_gnss(self, msg): """ Publish GNSS to Autoware """ self.publisher_map.publish(msg) - + def main(args=None): carla_vehicle_interface = CarlaVehicleInterface() -if __name__ == '__main__': - main() \ No newline at end of file + +if __name__ == "__main__": + main() diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md index 856700225f228..db5f83d412201 100644 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md @@ -1,3 +1,3 @@ # carla_sensor_kit_launch -A sensor kit configurations for CARLA Simulator +A sensor kit configurations for CARLA Simulator diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml index a5381436ebbc4..0cb769d8fdd44 100644 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py index e41aeef657456..11634a22736b0 100644 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py @@ -26,7 +26,6 @@ def launch_setup(context, *args, **kwargs): - # set concat filter as a component concat_component = ComposableNode( package="pointcloud_preprocessor", @@ -79,7 +78,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - launch_arguments = [] def add_launch_arg(name: str, default_value=None): diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py index 0b27efb5d7099..f3186d4db43db 100644 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py +++ b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py @@ -149,7 +149,7 @@ def create_parameter_dict(*args): namespace="", parameters=[ {"input_topic": "mirror_cropped/pointcloud_ex"}, - {"output_topic": "rectified/pointcloud_ex"} + {"output_topic": "rectified/pointcloud_ex"}, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -164,7 +164,9 @@ def create_parameter_dict(*args): distortion_relay_loader = LoadComposableNodes( composable_node_descriptions=[distortion_relay_component], target_container=container, - condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")), + condition=launch.conditions.UnlessCondition( + LaunchConfiguration("use_distortion_corrector") + ), ) return [container, distortion_loader, distortion_relay_loader] diff --git a/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt b/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt index d83a30d877e08..2ecfd1ee9aa26 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt +++ b/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt @@ -56,7 +56,7 @@ if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) ) endif() -rclcpp_components_register_node(pointcloud_interface +rclcpp_components_register_node(pointcloud_interface PLUGIN "PointCloudInterface" EXECUTABLE pointcloud_interface_node ) @@ -66,4 +66,3 @@ ament_auto_package( INSTALL_TO_SHARE launch ) - diff --git a/bridge/CARLA_Autoware/pointcloud_interface/README.md b/bridge/CARLA_Autoware/pointcloud_interface/README.md index b292d1b6c1dcf..f04e4c0dd7216 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/README.md +++ b/bridge/CARLA_Autoware/pointcloud_interface/README.md @@ -1,4 +1,3 @@ # carla_pointcloud_interface Convert Simple point cloud message to Autoware recognized Extended cloud message. - diff --git a/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp b/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp index 335a9ae4b46c3..d88e971e6635c 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp +++ b/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp @@ -1,21 +1,24 @@ -#ifndef MODEL_LOG_NODE_HPP_ -#define MODEL_LOG_NODE_HPP_ +#ifndef POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ +#define POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ -#include -#include #include "rclcpp/rclcpp.hpp" -#include + #include + +#include #include #include +#include +#include + class PointCloudInterface : public rclcpp::Node { public: - explicit PointCloudInterface(const rclcpp::NodeOptions & node_options); - virtual ~PointCloudInterface(); - + explicit PointCloudInterface(const rclcpp::NodeOptions & node_options); + virtual ~PointCloudInterface(); + private: std::shared_ptr tf_buffer_; rclcpp::Publisher::SharedPtr velodyne_points_raw; @@ -28,4 +31,4 @@ class PointCloudInterface : public rclcpp::Node void setupTF(); }; -#endif \ No newline at end of file +#endif // POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ diff --git a/bridge/CARLA_Autoware/pointcloud_interface/package.xml b/bridge/CARLA_Autoware/pointcloud_interface/package.xml index 0aec53de9c4a4..e95d83b4ff3f2 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/package.xml +++ b/bridge/CARLA_Autoware/pointcloud_interface/package.xml @@ -3,21 +3,23 @@ pointcloud_interface 1.0.0 - Convert CARLA 4D point cloud message to Autoware XYZIRADT cloud message + Convert CARLA 4D point cloud message to Autoware XYZIRADT cloud message Muhammad Raditya Giovanni Apache License 2.0 ament_cmake_auto angles - diagnostic_updater - autoware_auto_vehicle_msgs autoware_auto_control_msgs + autoware_auto_vehicle_msgs + cv_bridge + diagnostic_updater geometry_msgs + image_transport libpcl-all-dev message_filters - pcl_ros pcl_conversions + pcl_ros rclcpp rclcpp_components sensor_msgs @@ -26,9 +28,7 @@ velodyne_msgs visualization_msgs yaml-cpp - cv_bridge - image_transport - + ament_cmake diff --git a/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp b/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp index b56b1424e555c..1540d4c2773ca 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp +++ b/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp @@ -1,24 +1,23 @@ #include "pointcloud_interface/pointcloud_interface_node.hpp" + +#include + #include #include -#include -#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_top->publish(transformed_cloud); - velodyne_points_con->publish(transformed_cloud); - velodyne_points_raw->publish(transformed_cloud); - } - } - + { + 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_top->publish(transformed_cloud); + velodyne_points_con->publish(transformed_cloud); + velodyne_points_raw->publish(transformed_cloud); + } + } } void PointCloudInterface::setupTF() @@ -29,26 +28,23 @@ void PointCloudInterface::setupTF() PointCloudInterface::~PointCloudInterface() { - } 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()); - velodyne_points_top = - this->create_publisher("/sensing/lidar/top/outlier_filtered/pointcloud", rclcpp::SensorDataQoS()); - velodyne_points_con = - this->create_publisher("/sensing/lidar/concatenated/pointcloud", rclcpp::SensorDataQoS()); - } + 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()); + velodyne_points_top = this->create_publisher( + "/sensing/lidar/top/outlier_filtered/pointcloud", rclcpp::SensorDataQoS()); + velodyne_points_con = this->create_publisher( + "/sensing/lidar/concatenated/pointcloud", rclcpp::SensorDataQoS()); + } } #include "rclcpp_components/register_node_macro.hpp" From db2c29cb51a84f2277c900ff1b7d9b1b6373bd93 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 15 Mar 2024 16:11:21 +0900 Subject: [PATCH 03/12] minor fixes --- .../CARLA_Autoware/GNSS_Interface/README.md | 3 - .../launch/gnss_interface.launch.xml | 12 - bridge/CARLA_Autoware/LICENSE | 21 -- .../.markdown-link-check.json | 13 -- .../.markdownlint.yaml | 9 - .../.pre-commit-config-optional.yaml | 6 - .../.pre-commit-config.yaml | 68 ------ .../carla_sensor_kit_launch/.prettierignore | 2 - .../carla_sensor_kit_launch/.prettierrc.yaml | 20 -- .../carla_sensor_kit_launch/.yamllint.yaml | 23 -- .../CODE_OF_CONDUCT.md | 132 ----------- .../carla_sensor_kit_launch/CONTRIBUTING.md | 3 - .../carla_sensor_kit_launch/DISCLAIMER.md | 46 ---- .../carla_sensor_kit_launch/LICENSE | 201 ----------------- .../carla_sensor_kit_launch/NOTICE | 8 - .../carla_sensor_kit_launch/README.md | 3 - .../build_depends.repos | 9 - .../CMakeLists.txt | 11 - .../config/sensor_kit_calibration.yaml | 92 -------- .../config/sensors_calibration.yaml | 15 -- .../carla_sensor_kit_description/package.xml | 17 -- .../urdf/sensor_kit.xacro | 209 ----------------- .../urdf/sensors.xacro | 30 --- .../carla_sensor_kit_launch/CMakeLists.txt | 16 -- .../sensor_kit.param.yaml | 0 .../data/traffic_light_camera.yaml | 45 ---- .../launch/imu.launch.xml | 14 -- .../launch/lidar.launch.xml | 43 ---- .../launch/pointcloud_preprocessor.launch.py | 108 --------- .../launch/sensing.launch.xml | 33 --- .../carla_sensor_kit_launch/package.xml | 26 --- .../common_carla_sensor_launch/CMakeLists.txt | 14 -- .../launch/velodyne_lidar.launch.xml | 12 - .../launch/velodyne_node_container.launch.py | 210 ------------------ .../common_carla_sensor_launch/package.xml | 22 -- .../carla_sensor_kit_launch/setup.cfg | 15 -- .../pointcloud_interface/README.md | 3 - .../launch/pointcloud_interface.xml | 3 - .../carla_autoware/CMakeLists.txt | 0 .../CARLA_Autoware/carla_autoware}/README.md | 0 .../carla_autoware/config/objects.json | 0 .../launch/carla_autoware.launch.xml | 4 +- .../launch/carla_ros.launch.xml | 0 .../launch/e2e_simulator.launch.xml | 0 .../CARLA_Autoware/carla_autoware/package.xml | 2 - .../carla_autoware/resource/carla_autoware | 0 .../CARLA_Autoware/carla_autoware/setup.cfg | 0 .../CARLA_Autoware/carla_autoware/setup.py | 3 - .../src/carla_autoware/carla_autoware.py | 25 +-- .../carla_gnss_interface}/CMakeLists.txt | 12 +- .../carla_gnss_interface/README.md | 7 + .../carla_gnss_interface_node.hpp | 7 +- .../launch/gnss_interface.launch.xml | 5 + .../carla_gnss_interface}/package.xml | 2 +- .../carla_gnss_interface_node.cpp | 6 +- .../CMakeLists.txt | 12 +- .../carla_pointcloud_interface/README.md | 11 + .../carla_pointcloud_interface_node.hpp | 8 +- .../launch/carla_pointcloud_interface.xml | 3 + .../carla_pointcloud_interface}/package.xml | 2 +- .../carla_pointcloud_interface_node.cpp | 4 +- 61 files changed, 63 insertions(+), 1567 deletions(-) delete mode 100644 bridge/CARLA_Autoware/GNSS_Interface/README.md delete mode 100644 bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml delete mode 100644 bridge/CARLA_Autoware/LICENSE delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml delete mode 100644 bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg delete mode 100644 bridge/CARLA_Autoware/pointcloud_interface/README.md delete mode 100644 bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml rename {bridge => simulator}/CARLA_Autoware/carla_autoware/CMakeLists.txt (100%) rename {bridge/CARLA_Autoware => simulator/CARLA_Autoware/carla_autoware}/README.md (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/config/objects.json (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml (69%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/package.xml (99%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/resource/carla_autoware (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/setup.cfg (100%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/setup.py (96%) rename {bridge => simulator}/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py (91%) rename {bridge/CARLA_Autoware/GNSS_Interface => simulator/CARLA_Autoware/carla_gnss_interface}/CMakeLists.txt (67%) create mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/README.md rename bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp => simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp (90%) create mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml rename {bridge/CARLA_Autoware/GNSS_Interface => simulator/CARLA_Autoware/carla_gnss_interface}/package.xml (96%) rename bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp => simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp (96%) rename {bridge/CARLA_Autoware/pointcloud_interface => simulator/CARLA_Autoware/carla_pointcloud_interface}/CMakeLists.txt (80%) create mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/README.md rename bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp => simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp (77%) create mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml rename {bridge/CARLA_Autoware/pointcloud_interface => simulator/CARLA_Autoware/carla_pointcloud_interface}/package.xml (96%) rename bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp => simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp (94%) diff --git a/bridge/CARLA_Autoware/GNSS_Interface/README.md b/bridge/CARLA_Autoware/GNSS_Interface/README.md deleted file mode 100644 index b3d5a12fa8142..0000000000000 --- a/bridge/CARLA_Autoware/GNSS_Interface/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# gnss Interface Node - -Convert GNSS CARLA messages to pose and pose with covariance diff --git a/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml b/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml deleted file mode 100644 index 556e94f2821b1..0000000000000 --- a/bridge/CARLA_Autoware/GNSS_Interface/launch/gnss_interface.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/LICENSE b/bridge/CARLA_Autoware/LICENSE deleted file mode 100644 index b3c8c7e3fc138..0000000000000 --- a/bridge/CARLA_Autoware/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2024 Giovanni Muhammad Raditya - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json deleted file mode 100644 index dec3db1ad1c8d..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdown-link-check.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "aliveStatusCodes": [200, 206, 403], - "ignorePatterns": [ - { - "pattern": "^http://localhost" - }, - { - "pattern": "^https://github.com/.*/discussions/new" - } - ], - "retryOn429": true, - "retryCount": 10 -} diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml deleted file mode 100644 index df1f518dc0d48..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.markdownlint.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules. -default: true -MD013: false -MD024: - siblings_only: true -MD033: false -MD041: false -MD046: false -MD049: false diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml deleted file mode 100644 index a805f1201c414..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config-optional.yaml +++ /dev/null @@ -1,6 +0,0 @@ -repos: - - repo: https://github.com/tcort/markdown-link-check - rev: v3.10.0 - hooks: - - id: markdown-link-check - args: [--config=.markdown-link-check.json] diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml deleted file mode 100644 index 46b8247bde927..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.pre-commit-config.yaml +++ /dev/null @@ -1,68 +0,0 @@ -ci: - autofix_commit_msg: "ci(pre-commit): autofix" - autoupdate_commit_msg: "ci(pre-commit): autoupdate" - -repos: - - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 - hooks: - - id: check-json - - id: check-merge-conflict - - id: check-toml - - id: check-xml - - id: check-yaml - - id: detect-private-key - - id: end-of-file-fixer - - id: mixed-line-ending - - id: trailing-whitespace - args: [--markdown-linebreak-ext=md] - - - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.31.1 - hooks: - - id: markdownlint - args: [-c, .markdownlint.yaml, --fix] - - - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.7.1 - hooks: - - id: prettier - - - repo: https://github.com/adrienverge/yamllint - rev: v1.26.3 - hooks: - - id: yamllint - - - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.7.1 - hooks: - - id: flake8-ros - - id: prettier-xacro - - id: prettier-launch-xml - - id: prettier-package-xml - - id: ros-include-guard - - id: sort-package-xml - - - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.4 - hooks: - - id: shellcheck - - - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.5.1-1 - hooks: - - id: shfmt - args: [-w, -s, -i=4] - - - repo: https://github.com/pycqa/isort - rev: 5.10.1 - hooks: - - id: isort - - - repo: https://github.com/psf/black - rev: 22.6.0 - hooks: - - id: black - args: [--line-length=100] - -exclude: .svg diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore deleted file mode 100644 index a3c34d00a1377..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierignore +++ /dev/null @@ -1,2 +0,0 @@ -*.param.yaml -*.rviz diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml deleted file mode 100644 index e29bf32762769..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.prettierrc.yaml +++ /dev/null @@ -1,20 +0,0 @@ -printWidth: 100 -tabWidth: 2 -overrides: - - files: package.xml - options: - printWidth: 1000 - xmlSelfClosingSpace: false - xmlWhitespaceSensitivity: ignore - - - files: "*.launch.xml" - options: - printWidth: 200 - xmlSelfClosingSpace: false - xmlWhitespaceSensitivity: ignore - - - files: "*.xacro" - options: - printWidth: 200 - xmlSelfClosingSpace: false - xmlWhitespaceSensitivity: ignore diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml deleted file mode 100644 index 6228c70f02da9..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/.yamllint.yaml +++ /dev/null @@ -1,23 +0,0 @@ -extends: default - -ignore: | - .clang-tidy - *.param.yaml - -rules: - braces: - level: error - max-spaces-inside: 1 # To format with Prettier - comments: - level: error - min-spaces-from-content: 1 # To be compatible with C++ and Python - document-start: - level: error - present: false # Don't need document start markers - line-length: disable # Delegate to Prettier - truthy: - level: error - check-keys: false # To allow 'on' of GitHub Actions - quoted-strings: - level: error - required: only-when-needed # To keep consistent style diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md deleted file mode 100644 index c493cad4da591..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CODE_OF_CONDUCT.md +++ /dev/null @@ -1,132 +0,0 @@ -# Contributor Covenant Code of Conduct - -## Our Pledge - -We as members, contributors, and leaders pledge to make participation in our -community a harassment-free experience for everyone, regardless of age, body -size, visible or invisible disability, ethnicity, sex characteristics, gender -identity and expression, level of experience, education, socio-economic status, -nationality, personal appearance, race, caste, color, religion, or sexual -identity and orientation. - -We pledge to act and interact in ways that contribute to an open, welcoming, -diverse, inclusive, and healthy community. - -## Our Standards - -Examples of behavior that contributes to a positive environment for our -community include: - -- Demonstrating empathy and kindness toward other people -- Being respectful of differing opinions, viewpoints, and experiences -- Giving and gracefully accepting constructive feedback -- Accepting responsibility and apologizing to those affected by our mistakes, - and learning from the experience -- Focusing on what is best not just for us as individuals, but for the overall - community - -Examples of unacceptable behavior include: - -- The use of sexualized language or imagery, and sexual attention or advances of - any kind -- Trolling, insulting or derogatory comments, and personal or political attacks -- Public or private harassment -- Publishing others' private information, such as a physical or email address, - without their explicit permission -- Other conduct which could reasonably be considered inappropriate in a - professional setting - -## Enforcement Responsibilities - -Community leaders are responsible for clarifying and enforcing our standards of -acceptable behavior and will take appropriate and fair corrective action in -response to any behavior that they deem inappropriate, threatening, offensive, -or harmful. - -Community leaders have the right and responsibility to remove, edit, or reject -comments, commits, code, wiki edits, issues, and other contributions that are -not aligned to this Code of Conduct, and will communicate reasons for moderation -decisions when appropriate. - -## Scope - -This Code of Conduct applies within all community spaces, and also applies when -an individual is officially representing the community in public spaces. -Examples of representing our community include using an official e-mail address, -posting via an official social media account, or acting as an appointed -representative at an online or offline event. - -## Enforcement - -Instances of abusive, harassing, or otherwise unacceptable behavior may be -reported to the community leaders responsible for enforcement at -conduct@autoware.org. -All complaints will be reviewed and investigated promptly and fairly. - -All community leaders are obligated to respect the privacy and security of the -reporter of any incident. - -## Enforcement Guidelines - -Community leaders will follow these Community Impact Guidelines in determining -the consequences for any action they deem in violation of this Code of Conduct: - -### 1. Correction - -**Community Impact**: Use of inappropriate language or other behavior deemed -unprofessional or unwelcome in the community. - -**Consequence**: A private, written warning from community leaders, providing -clarity around the nature of the violation and an explanation of why the -behavior was inappropriate. A public apology may be requested. - -### 2. Warning - -**Community Impact**: A violation through a single incident or series of -actions. - -**Consequence**: A warning with consequences for continued behavior. No -interaction with the people involved, including unsolicited interaction with -those enforcing the Code of Conduct, for a specified period of time. This -includes avoiding interactions in community spaces as well as external channels -like social media. Violating these terms may lead to a temporary or permanent -ban. - -### 3. Temporary Ban - -**Community Impact**: A serious violation of community standards, including -sustained inappropriate behavior. - -**Consequence**: A temporary ban from any sort of interaction or public -communication with the community for a specified period of time. No public or -private interaction with the people involved, including unsolicited interaction -with those enforcing the Code of Conduct, is allowed during this period. -Violating these terms may lead to a permanent ban. - -### 4. Permanent Ban - -**Community Impact**: Demonstrating a pattern of violation of community -standards, including sustained inappropriate behavior, harassment of an -individual, or aggression toward or disparagement of classes of individuals. - -**Consequence**: A permanent ban from any sort of public interaction within the -community. - -## Attribution - -This Code of Conduct is adapted from the [Contributor Covenant][homepage], -version 2.1, available at -[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. - -Community Impact Guidelines were inspired by -[Mozilla's code of conduct enforcement ladder][mozilla coc]. - -For answers to common questions about this code of conduct, see the FAQ at -[https://www.contributor-covenant.org/faq][faq]. Translations are available at -[https://www.contributor-covenant.org/translations][translations]. - -[homepage]: https://www.contributor-covenant.org -[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html -[mozilla coc]: https://github.com/mozilla/diversity -[faq]: https://www.contributor-covenant.org/faq -[translations]: https://www.contributor-covenant.org/translations diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md deleted file mode 100644 index 22c7ee28e8d9a..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/CONTRIBUTING.md +++ /dev/null @@ -1,3 +0,0 @@ -# Contributing - -See . diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md deleted file mode 100644 index 1b5a9bbe4d981..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/DISCLAIMER.md +++ /dev/null @@ -1,46 +0,0 @@ -DISCLAIMER - -“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0. -This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with -the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents -specified in this disclaimer below and will be deemed to consent to this -disclaimer without any objection upon utilizing or downloading Autoware. - -Disclaimer and Waiver of Warranties - -1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, - EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) - including but not limited to any representation or warranty (i) of fitness or - suitability for a particular purpose contemplated by the Users, (ii) of the - expected functions, commercial value, accuracy, or usefulness of the Service, - (iii) that the use by the Users of the Service complies with the laws and - regulations applicable to the Users or any internal rules established by - industrial organizations, (iv) that the Service will be free of interruption or - defects, (v) of the non-infringement of any third party's right and (vi) the - accuracy of the content of the Services and the software itself. - -2. The Autoware Foundation shall not be liable for any damage incurred by the - User that are attributable to the Autoware Foundation for any reasons - whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR - INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. - -3. A User shall be entirely responsible for the content posted by the User and - its use of any content of the Service or the Website. If the User is held - responsible in a civil action such as a claim for damages or even in a criminal - case, the Autoware Foundation and member companies, governments and academic & - non-profit organizations and their directors, officers, employees and agents - (collectively, the “Indemnified Parties”) shall be completely discharged from - any rights or assertions the User may have against the Indemnified Parties, or - from any legal action, litigation or similar procedures. - -Indemnity - -A User shall indemnify and hold the Indemnified Parties harmless from any of -their damages, losses, liabilities, costs or expenses (including attorneys' -fees or criminal compensation), or any claims or demands made against the -Indemnified Parties by any third party, due to or arising out of, or in -connection with utilizing Autoware (including the representations and -warranties), the violation of applicable Product Liability Law of each country -(including criminal case) or violation of any applicable laws by the Users, or -the content posted by the User or its use of any content of the Service or the -Website. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE b/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE deleted file mode 100644 index 261eeb9e9f8b2..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE b/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE deleted file mode 100644 index 60fbbe96174e5..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/NOTICE +++ /dev/null @@ -1,8 +0,0 @@ -carla_sensor_kit_launch -Copyright 2021 The Autoware Foundation - -This product includes software developed at -The Autoware Foundation (https://www.autoware.org/). - -This product includes code developed by TIER IV. -Copyright 2020 TIER IV, Inc. diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md b/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md deleted file mode 100644 index db5f83d412201..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# carla_sensor_kit_launch - -A sensor kit configurations for CARLA Simulator diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos b/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos deleted file mode 100644 index 08e78b602a8d0..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/build_depends.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - autoware_common: - type: git - url: https://github.com/autowarefoundation/autoware_common.git - version: main - sensor_component_description: - type: git - url: https://github.com/tier4/sensor_component_description.git - version: main diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt deleted file mode 100644 index b2b320a645d80..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(carla_sensor_kit_description) - -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - -ament_auto_package(INSTALL_TO_SHARE - urdf - config -) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml deleted file mode 100644 index aa2aeb5b3a42e..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensor_kit_calibration.yaml +++ /dev/null @@ -1,92 +0,0 @@ -sensor_kit_base_link: - camera0/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - camera1/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - camera2/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - camera3/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - camera4/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - camera5/camera_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - traffic_light_right_camera/camera_link: - x: -0.7 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - traffic_light_left_camera/camera_link: - x: 0.7 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - velodyne_top_base_link: - x: 0.0 - y: 0.0 - z: 1.0 - roll: 0.0 - pitch: 0.0 - yaw: 0 - velodyne_left_base_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - velodyne_right_base_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - gnss_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - tamagawa/imu_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml deleted file mode 100644 index c25cc52357e97..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/config/sensors_calibration.yaml +++ /dev/null @@ -1,15 +0,0 @@ -base_link: - sensor_kit_base_link: - x: 0.0 - y: 0.0 - z: 1.6 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 - velodyne_rear_base_link: - x: 0.0 - y: 0.0 - z: 0.0 - roll: 0.0 - pitch: 0.0 - yaw: 0.0 diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml deleted file mode 100644 index 638e73fb15972..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - carla_sensor_kit_description - 0.1.0 - The carla_sensor_kit_description package - Hatem Darweesh - Apache License 2.0 - - ament_cmake_auto - - velodyne_description - - - ament_cmake - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro deleted file mode 100644 index c40b383563186..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,209 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro deleted file mode 100644 index cf7966f010927..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_description/urdf/sensors.xacro +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt deleted file mode 100644 index 0ba7432e6d898..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(carla_sensor_kit_launch) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE - launch - data - config -) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/config/diagnostic_aggregator/sensor_kit.param.yaml deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml deleted file mode 100644 index 5525f8189b213..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/data/traffic_light_camera.yaml +++ /dev/null @@ -1,45 +0,0 @@ -image_width: 1920 -image_height: 1080 -camera_name: traffic_light/camera -camera_matrix: - rows: 3 - cols: 3 - data: - [ - 2410.755261, - 0.000000, - 922.621401, - 0.000000, - 2403.573140, - 534.752500, - 0.000000, - 0.000000, - 1.000000, - ] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000] -rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] -projection_matrix: - rows: 3 - cols: 4 - data: - [ - 2370.254883, - 0.000000, - 920.136018, - 0.000000, - 0.000000, - 2388.885254, - 535.599668, - 0.000000, - 0.000000, - 0.000000, - 1.000000, - 0.000000, - ] diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml deleted file mode 100644 index 0cb769d8fdd44..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/imu.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml deleted file mode 100644 index 472051f25de01..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/lidar.launch.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py deleted file mode 100644 index 11634a22736b0..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py +++ /dev/null @@ -1,108 +0,0 @@ -# Copyright 2020 Tier IV, Inc. 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. - - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode - - -def launch_setup(context, *args, **kwargs): - # set concat filter as a component - concat_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("output", "concatenated/pointcloud"), - ], - parameters=[ - { - "input_topics": [ - "/sensing/lidar/top/outlier_filtered/pointcloud", - "/sensing/lidar/left/outlier_filtered/pointcloud", - "/sensing/lidar/right/outlier_filtered/pointcloud", - ], - "output_frame": LaunchConfiguration("base_frame"), - "timeout_sec": 0.01, - "input_twist_topic_type": "twist", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[], - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("container_name") - ) - - # load concat or passthrough filter - concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component], - target_container=target_container, - condition=IfCondition(LaunchConfiguration("use_concat_filter")), - ) - - return [container, concat_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None): - launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) - - add_launch_arg("base_frame", "base_link") - add_launch_arg("use_multithread", "False") - add_launch_arg("use_intra_process", "False") - add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml deleted file mode 100644 index 02cc9df228c29..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/launch/sensing.launch.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml deleted file mode 100644 index 1d2e1dece2c20..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/carla_sensor_kit_launch/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - carla_sensor_kit_launch - 0.1.0 - The carla_sensor_kit_launch package - Hatem Darweesh - Apache License 2.0 - - ament_cmake_auto - - common_sensor_launch - gnss_poser - pointcloud_preprocessor - tamagawa_imu_driver - topic_tools - ublox_gps - usb_cam - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt deleted file mode 100644 index 76585c6207b3f..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(common_carla_sensor_launch) - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE - launch -) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml deleted file mode 100644 index 4dff1789703e6..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_lidar.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py deleted file mode 100644 index f3186d4db43db..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/launch/velodyne_node_container.launch.py +++ /dev/null @@ -1,210 +0,0 @@ -# Copyright 2020 Tier IV, Inc. 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. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_vehicle_info(context): - # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support - # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py - gp = context.launch_configurations.get("ros_params", {}) - if not gp: - gp = dict(context.launch_configurations.get("global_params", {})) - p = {} - p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] - p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] - p["min_longitudinal_offset"] = -gp["rear_overhang"] - p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] - p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) - p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] - p["min_height_offset"] = 0.0 - p["max_height_offset"] = gp["vehicle_height"] - return p - - -def get_vehicle_mirror_info(context): - path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - -def launch_setup(context, *args, **kwargs): - def create_parameter_dict(*args): - result = {} - for x in args: - result[x] = LaunchConfiguration(x) - return result - - nodes = [] - - cropbox_parameters = create_parameter_dict("input_frame", "output_frame") - cropbox_parameters["negative"] = True - - vehicle_info = get_vehicle_info(context) - cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] - cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] - cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] - cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_self", - remappings=[ - ("input", "pointcloud_raw_ex"), - ("output", "self_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - mirror_info = get_vehicle_mirror_info(context) - cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"] - cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"] - cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"] - cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"] - cropbox_parameters["min_z"] = mirror_info["min_height_offset"] - cropbox_parameters["max_z"] = mirror_info["max_height_offset"] - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_mirror", - remappings=[ - ("input", "self_cropped/pointcloud_ex"), - ("output", "mirror_cropped/pointcloud_ex"), - ], - parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - nodes.append( - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", - name="ring_outlier_filter", - remappings=[ - ("input", "rectified/pointcloud_ex"), - ("output", "outlier_filtered/pointcloud"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - # set container to run all required components in the same process - container = ComposableNodeContainer( - # need unique name, otherwise all processes in same container and the node names then clash - name="velodyne_node_container", - namespace="pointcloud_preprocessor", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=nodes, - ) - - distortion_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::DistortionCorrectorComponent", - name="distortion_corrector_node", - remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), - ("~/input/imu", "/sensing/imu/imu_data"), - ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), - ("~/output/pointcloud", "rectified/pointcloud_ex"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - distortion_relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="pointcloud_distortion_relay", - namespace="", - parameters=[ - {"input_topic": "mirror_cropped/pointcloud_ex"}, - {"output_topic": "rectified/pointcloud_ex"}, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # one way to add a ComposableNode conditional on a launch argument to a - # container. The `ComposableNode` itself doesn't accept a condition - distortion_loader = LoadComposableNodes( - composable_node_descriptions=[distortion_component], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("use_distortion_corrector")), - ) - distortion_relay_loader = LoadComposableNodes( - composable_node_descriptions=[distortion_relay_component], - target_container=container, - condition=launch.conditions.UnlessCondition( - LaunchConfiguration("use_distortion_corrector") - ), - ) - - return [container, distortion_loader, distortion_relay_loader] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - # a default_value of None is equivalent to not passing that kwarg at all - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("base_frame", "base_link", "base frame id") - add_launch_arg("container_name", "velodyne_composable_node_container", "container name") - add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") - add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") - add_launch_arg( - "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" - ) - add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [set_container_executable, set_container_mt_executable] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml b/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml deleted file mode 100644 index c8028c196dc83..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/common_carla_sensor_launch/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - common_carla_sensor_launch - 0.1.0 - The common_carla_sensor_launch package - Hatem Darweesh - Apache License 2.0 - - ament_cmake_auto - - velodyne_driver - velodyne_monitor - velodyne_pointcloud - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg b/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg deleted file mode 100644 index 5214751c7ba6b..0000000000000 --- a/bridge/CARLA_Autoware/carla_sensor_kit_launch/setup.cfg +++ /dev/null @@ -1,15 +0,0 @@ -[flake8] -# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini -extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 -import-order-style = pep8 -max-line-length = 100 -show-source = true -statistics = true - -[isort] -profile=black -line_length=100 -force_sort_within_sections=true -force_single_line=true -reverse_relative=true -known_third_party=launch diff --git a/bridge/CARLA_Autoware/pointcloud_interface/README.md b/bridge/CARLA_Autoware/pointcloud_interface/README.md deleted file mode 100644 index f04e4c0dd7216..0000000000000 --- a/bridge/CARLA_Autoware/pointcloud_interface/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# carla_pointcloud_interface - -Convert Simple point cloud message to Autoware recognized Extended cloud message. diff --git a/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml b/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml deleted file mode 100644 index 8b6eb39bba2dd..0000000000000 --- a/bridge/CARLA_Autoware/pointcloud_interface/launch/pointcloud_interface.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt b/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/CMakeLists.txt rename to simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt diff --git a/bridge/CARLA_Autoware/README.md b/simulator/CARLA_Autoware/carla_autoware/README.md similarity index 100% rename from bridge/CARLA_Autoware/README.md rename to simulator/CARLA_Autoware/carla_autoware/README.md diff --git a/bridge/CARLA_Autoware/carla_autoware/config/objects.json b/simulator/CARLA_Autoware/carla_autoware/config/objects.json similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/config/objects.json rename to simulator/CARLA_Autoware/carla_autoware/config/objects.json diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml similarity index 69% rename from bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml rename to simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml index 6e82013a84531..1e71edcd8eb39 100644 --- a/bridge/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml +++ b/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml @@ -4,7 +4,7 @@ - - + + diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml rename to simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml diff --git a/bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml rename to simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml diff --git a/bridge/CARLA_Autoware/carla_autoware/package.xml b/simulator/CARLA_Autoware/carla_autoware/package.xml similarity index 99% rename from bridge/CARLA_Autoware/carla_autoware/package.xml rename to simulator/CARLA_Autoware/carla_autoware/package.xml index 2d50f4742f2eb..4cad00c7420fa 100644 --- a/bridge/CARLA_Autoware/carla_autoware/package.xml +++ b/simulator/CARLA_Autoware/carla_autoware/package.xml @@ -26,8 +26,6 @@ tf2_ros transform3d - - ament_cmake ament_lint_auto diff --git a/bridge/CARLA_Autoware/carla_autoware/resource/carla_autoware b/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/resource/carla_autoware rename to simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.cfg b/simulator/CARLA_Autoware/carla_autoware/setup.cfg similarity index 100% rename from bridge/CARLA_Autoware/carla_autoware/setup.cfg rename to simulator/CARLA_Autoware/carla_autoware/setup.cfg diff --git a/bridge/CARLA_Autoware/carla_autoware/setup.py b/simulator/CARLA_Autoware/carla_autoware/setup.py similarity index 96% rename from bridge/CARLA_Autoware/carla_autoware/setup.py rename to simulator/CARLA_Autoware/carla_autoware/setup.py index c02295fdd714b..139f98586afc6 100644 --- a/bridge/CARLA_Autoware/carla_autoware/setup.py +++ b/simulator/CARLA_Autoware/carla_autoware/setup.py @@ -1,6 +1,3 @@ -""" -Setup for carla_manual_control -""" from glob import glob import os diff --git a/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py similarity index 91% rename from bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py rename to simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py index 41aab41397860..6b00e37c67e86 100644 --- a/bridge/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py @@ -1,4 +1,3 @@ -import math import threading from autoware_auto_control_msgs.msg import AckermannControlCommand @@ -14,8 +13,6 @@ from rclpy.qos import QoSProfile from sensor_msgs.msg import Imu from sensor_msgs.msg import PointCloud2 -from sensor_msgs_py.point_cloud2 import create_cloud -from transforms3d.euler import quat2euler class CarlaVehicleInterface(Node): @@ -81,9 +78,7 @@ def __init__(self): ) def ego_status_callback(self, in_status): - """ - Callback function for CARLA Ego Vehicle Status to AUTOWARE - """ + """Convert and publish CARLA Ego Vehicle Status to AUTOWARE.""" out_vel_state = VelocityReport() out_steering_state = SteeringReport() out_ctrl_mode = ControlModeReport() @@ -111,9 +106,7 @@ def ego_status_callback(self, in_status): self.pub_gear_state.publish(out_gear_state) def control_callback(self, in_cmd): - """ - Callback function for CARLA Control - """ + """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 @@ -134,28 +127,22 @@ def control_callback(self, in_cmd): self.pub_control.publish(out_cmd) def publish_lidar(self, lidar_msg): - """ - Publish LIDAR to Interface - """ + """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 - """ + """Publish IMU to Autoware.""" imu_msg.header.frame_id = "tamagawa/imu_link" self.vehicle_imu_publisher.publish(imu_msg) def publish_gnss(self, msg): - """ - Publish GNSS to Autoware - """ + """Publish GNSS to Autoware.""" self.publisher_map.publish(msg) def main(args=None): - carla_vehicle_interface = CarlaVehicleInterface() + CarlaVehicleInterface() if __name__ == "__main__": diff --git a/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt similarity index 67% rename from bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt rename to simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt index a5129ffb3d2c7..75d8490890e4c 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(gnss_interface) +project(carla_gnss_interface) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() @@ -20,15 +20,15 @@ ament_auto_find_build_dependencies() find_package(PkgConfig REQUIRED) pkg_check_modules(PROJ REQUIRED proj) -ament_auto_add_library(gnss_interface SHARED - src/gnss_interface/gnss_interface_node.cpp +ament_auto_add_library(carla_gnss_interface SHARED + src/carla_gnss_interface/carla_gnss_interface_node.cpp ) -target_link_libraries(gnss_interface ${PROJ_LIBRARIES}) +target_link_libraries(carla_gnss_interface ${PROJ_LIBRARIES}) -rclcpp_components_register_node(gnss_interface +rclcpp_components_register_node(carla_gnss_interface PLUGIN "GnssInterface" - EXECUTABLE gnss_interface_node + EXECUTABLE carla_gnss_interface_node ) ament_auto_package( diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/README.md b/simulator/CARLA_Autoware/carla_gnss_interface/README.md new file mode 100644 index 0000000000000..8e3ec20befe68 --- /dev/null +++ b/simulator/CARLA_Autoware/carla_gnss_interface/README.md @@ -0,0 +1,7 @@ +# 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/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp b/simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp similarity index 90% rename from bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp rename to simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp index 034af17d42a12..fec3c7c909dd8 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/include/gnss_interface/gnss_interface_node.hpp +++ b/simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp @@ -1,6 +1,7 @@ +// Copyright 2024 Raditya. -#ifndef GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ -#define GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ +#ifndef CARLA_GNSS_INTERFACE__CARLA_GNSS_INTERFACE_NODE_HPP_ +#define CARLA_GNSS_INTERFACE__CARLA_GNSS_INTERFACE_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -101,4 +102,4 @@ class MappingUtils } // namespace gnss } // namespace interface -#endif // GNSS_INTERFACE__GNSS_INTERFACE_NODE_HPP_ +#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 new file mode 100644 index 0000000000000..6faf90f770022 --- /dev/null +++ b/simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/bridge/CARLA_Autoware/GNSS_Interface/package.xml b/simulator/CARLA_Autoware/carla_gnss_interface/package.xml similarity index 96% rename from bridge/CARLA_Autoware/GNSS_Interface/package.xml rename to simulator/CARLA_Autoware/carla_gnss_interface/package.xml index 396359de45f38..74aeac6e9c643 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/package.xml +++ b/simulator/CARLA_Autoware/carla_gnss_interface/package.xml @@ -1,7 +1,7 @@ - gnss_interface + carla_gnss_interface 1.0.0 Convert GNSS CARLA messages to pose and pose with covariance Muhammad Raditya Giovanni diff --git a/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp b/simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp similarity index 96% rename from bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp rename to simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp index 9b5543d458796..0496371a66be9 100644 --- a/bridge/CARLA_Autoware/GNSS_Interface/src/gnss_interface/gnss_interface_node.cpp +++ b/simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp @@ -1,4 +1,6 @@ -#include "gnss_interface/gnss_interface_node.hpp" +// Copyright 2024 Raditya. + +#include "carla_gnss_interface/carla_gnss_interface_node.hpp" #include @@ -7,8 +9,6 @@ namespace interface namespace gnss { -using namespace std; - MappingUtils::MappingUtils() { } diff --git a/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt similarity index 80% rename from bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt rename to simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt index 2ecfd1ee9aa26..43f49410ed151 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(pointcloud_interface) +project(carla_pointcloud_interface) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() @@ -44,21 +44,21 @@ include_directories( ${PCL_COMMON_INCLUDE_DIRS} ) -ament_auto_add_library(pointcloud_interface SHARED - src/pointcloud_interface/pointcloud_interface_node.cpp +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(pointcloud_interface PRIVATE + target_compile_definitions(carla_pointcloud_interface PRIVATE USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER ) endif() -rclcpp_components_register_node(pointcloud_interface +rclcpp_components_register_node(carla_pointcloud_interface PLUGIN "PointCloudInterface" - EXECUTABLE pointcloud_interface_node + EXECUTABLE carla_pointcloud_interface_node ) diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md b/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md new file mode 100644 index 0000000000000..c47a171bfc415 --- /dev/null +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/README.md @@ -0,0 +1,11 @@ +# 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/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp b/simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp similarity index 77% rename from bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp rename to simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp index d88e971e6635c..75be0b56e9dbe 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/include/pointcloud_interface/pointcloud_interface_node.hpp +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp @@ -1,6 +1,7 @@ +// Copyright 2024 Raditya. -#ifndef POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ -#define POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ +#ifndef CARLA_POINTCLOUD_INTERFACE__CARLA_POINTCLOUD_INTERFACE_NODE_HPP_ +#define CARLA_POINTCLOUD_INTERFACE__CARLA_POINTCLOUD_INTERFACE_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -10,6 +11,7 @@ #include #include +#include #include #include @@ -31,4 +33,4 @@ class PointCloudInterface : public rclcpp::Node void setupTF(); }; -#endif // POINTCLOUD_INTERFACE__POINTCLOUD_INTERFACE_NODE_HPP_ +#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 new file mode 100644 index 0000000000000..2b53ad1b2a50b --- /dev/null +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml @@ -0,0 +1,3 @@ + + + diff --git a/bridge/CARLA_Autoware/pointcloud_interface/package.xml b/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml similarity index 96% rename from bridge/CARLA_Autoware/pointcloud_interface/package.xml rename to simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml index e95d83b4ff3f2..5272a666d9ee0 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/package.xml +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml @@ -1,7 +1,7 @@ - pointcloud_interface + carla_pointcloud_interface 1.0.0 Convert CARLA 4D point cloud message to Autoware XYZIRADT cloud message Muhammad Raditya Giovanni diff --git a/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp b/simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp similarity index 94% rename from bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp rename to simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp index 1540d4c2773ca..b3981d9bfc3f8 100644 --- a/bridge/CARLA_Autoware/pointcloud_interface/src/pointcloud_interface/pointcloud_interface_node.cpp +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp @@ -1,4 +1,6 @@ -#include "pointcloud_interface/pointcloud_interface_node.hpp" +// Copyright 2024 Raditya. + +#include "carla_pointcloud_interface/carla_pointcloud_interface_node.hpp" #include From bc1bee56ffb25de1efd6664ea0723df28809bd80 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 15 Mar 2024 17:42:53 +0900 Subject: [PATCH 04/12] remove redundancy and added remap --- .../launch/carla_autoware.launch.xml | 7 +- .../launch/carla_ros.launch.xml | 2 +- .../launch/e2e_simulator.launch.xml | 84 ------------------- .../carla_autoware/resource/carla_autoware | 0 .../carla_gnss_interface_node.hpp | 14 +++- .../carla_gnss_interface_node.cpp | 14 +++- .../carla_pointcloud_interface_node.hpp | 18 +++- .../carla_pointcloud_interface_node.cpp | 23 ++--- 8 files changed, 59 insertions(+), 103 deletions(-) delete mode 100644 simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml delete mode 100644 simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware diff --git a/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml index 1e71edcd8eb39..54bf9db5019d3 100644 --- a/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml +++ b/simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml @@ -3,8 +3,11 @@ - + - + + + + diff --git a/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml index a3b1b892b68fb..1bed1146b4820 100644 --- a/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml +++ b/simulator/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml b/simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml deleted file mode 100644 index 8226a45c29088..0000000000000 --- a/simulator/CARLA_Autoware/carla_autoware/launch/e2e_simulator.launch.xml +++ /dev/null @@ -1,84 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware b/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware deleted file mode 100644 index e69de29bb2d1d..0000000000000 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 index fec3c7c909dd8..48b1aca5d0ec4 100644 --- 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 @@ -1,4 +1,16 @@ -// Copyright 2024 Raditya. +// 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_ 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 index 0496371a66be9..9b118e5762a9a 100644 --- 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 @@ -1,4 +1,16 @@ -// Copyright 2024 Raditya. +// 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" 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 index 75be0b56e9dbe..1498321c1fe7a 100644 --- 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 @@ -1,4 +1,16 @@ -// Copyright 2024 Raditya. +// 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_ @@ -23,10 +35,8 @@ class PointCloudInterface : public rclcpp::Node private: std::shared_ptr tf_buffer_; - rclcpp::Publisher::SharedPtr velodyne_points_raw; - rclcpp::Publisher::SharedPtr velodyne_points_top; - rclcpp::Publisher::SharedPtr velodyne_points_con; 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); 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 index b3981d9bfc3f8..187553da913eb 100644 --- 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 @@ -1,12 +1,21 @@ -// Copyright 2024 Raditya. +// 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 -#include - #include void PointCloudInterface::processScan(const sensor_msgs::msg::PointCloud2::SharedPtr scanMsg) @@ -15,8 +24,6 @@ void PointCloudInterface::processScan(const sensor_msgs::msg::PointCloud2::Share 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_top->publish(transformed_cloud); - velodyne_points_con->publish(transformed_cloud); velodyne_points_raw->publish(transformed_cloud); } } @@ -42,10 +49,6 @@ PointCloudInterface::PointCloudInterface(const rclcpp::NodeOptions & node_option setupTF(); velodyne_points_raw = this->create_publisher("/points_raw", rclcpp::SensorDataQoS()); - velodyne_points_top = this->create_publisher( - "/sensing/lidar/top/outlier_filtered/pointcloud", rclcpp::SensorDataQoS()); - velodyne_points_con = this->create_publisher( - "/sensing/lidar/concatenated/pointcloud", rclcpp::SensorDataQoS()); } } From 17e9b52a1419a43149c675c22d7d83e2fd738458 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 15 Mar 2024 17:54:22 +0900 Subject: [PATCH 05/12] fixed cmake by adding autoware_package --- simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt | 3 +++ simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt b/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt index deb641375684e..c51fe86d73706 100644 --- a/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt @@ -1,6 +1,9 @@ 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() diff --git a/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt index 75d8490890e4c..9715894c554ce 100644 --- a/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt @@ -1,5 +1,9 @@ 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() From 2ca6c7baf798773f0d11f7ebfef320bb1136989d Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Tue, 19 Mar 2024 12:39:48 +0900 Subject: [PATCH 06/12] encapsulate some function Signed-off-by: mraditya01 --- .../carla_gnss_interface_node.hpp | 35 +++++++----- .../carla_gnss_interface_node.cpp | 54 ++++++++++--------- .../carla_pointcloud_interface/CMakeLists.txt | 4 ++ 3 files changed, 55 insertions(+), 38 deletions(-) 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 index 48b1aca5d0ec4..06823c2971fb3 100644 --- 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 @@ -88,30 +88,37 @@ class GPSPoint class WayPoint { public: - GPSPoint pos; - WayPoint() {} + 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; + } +}; - 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; }; class MappingUtils { public: - MappingUtils(); - virtual ~MappingUtils(); + MappingUtils(); + virtual ~MappingUtils(); - static void llaToxyz( - const std::string & proj_str, const WayPoint & origin, const double & lat, const double & lon, - const double & alt, double & x_out, double & y_out, double & z_out); + static void llaToxyz(const MappingParameters ¶ms, 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/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 index 9b118e5762a9a..0079f4883eb9e 100644 --- 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 @@ -21,33 +21,29 @@ namespace interface namespace gnss { -MappingUtils::MappingUtils() -{ -} +MappingUtils::MappingUtils() {} -MappingUtils::~MappingUtils() -{ -} +MappingUtils::~MappingUtils() {} -void MappingUtils::llaToxyz( - const std::string & proj_str, const WayPoint & origin, const double & lat, const double & lon, - const double & alt, double & x_out, double & y_out, double & z_out) -{ - if (proj_str.size() < 8) return; +void MappingUtils::llaToxyz(const MappingParameters ¶ms, 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", proj_str.c_str(), NULL); + PJ_CONTEXT *C = proj_context_create(); + PJ *P = proj_create_crs_to_crs(C, "EPSG:4326", params.proj_str.c_str(), NULL); - if (P == 0) return; + if (P == nullptr) { + proj_context_destroy(C); + return; + } - PJ_COORD gps_degrees = proj_coord(lat, lon, alt, 0); - PJ_COORD xyz_out = proj_trans(P, PJ_FWD, gps_degrees); - x_out = xyz_out.enu.e + origin.pos.x; - y_out = xyz_out.enu.n + origin.pos.y; - z_out = xyz_out.enu.u + origin.pos.z; + 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); + proj_destroy(P); + proj_context_destroy(C); } } // namespace gnss } // namespace interface @@ -57,9 +53,19 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms geometry_msgs::msg::PoseStamped pose_; geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_; interface::gnss::WayPoint origin, p; - interface::gnss::MappingUtils::llaToxyz( - "+proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs", origin, - msg->latitude, msg->longitude, msg->altitude, p.pos.x, p.pos.y, p.pos.z); + // Create an instance of MappingUtils + interface::gnss::MappingUtils mappingUtils; + + // Create MappingParameters struct to hold the parameters + interface::gnss::MappingParameters params; + 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 + mappingUtils.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; diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt index 43f49410ed151..692423f6d8aa0 100644 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt @@ -1,4 +1,8 @@ cmake_minimum_required(VERSION 3.5) + +find_package(autoware_cmake REQUIRED) +autoware_package() + project(carla_pointcloud_interface) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) From 2e03c9bf04c4457e533f41834db53359728ef3eb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 19 Mar 2024 03:41:59 +0000 Subject: [PATCH 07/12] style(pre-commit): autofix --- .../carla_gnss_interface_node.hpp | 41 +++++++++--------- .../carla_gnss_interface_node.cpp | 43 +++++++++++-------- 2 files changed, 46 insertions(+), 38 deletions(-) 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 index 06823c2971fb3..47e6d3821e1cc 100644 --- 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 @@ -88,37 +88,38 @@ class GPSPoint 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; - } + 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; +struct MappingParameters +{ + std::string proj_str; + WayPoint org; + double lat; + double lon; + double alt; }; class MappingUtils { public: - MappingUtils(); - virtual ~MappingUtils(); + MappingUtils(); + virtual ~MappingUtils(); - static void llaToxyz(const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out); + static 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/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 index 0079f4883eb9e..468498b0d294a 100644 --- 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 @@ -21,29 +21,35 @@ namespace interface namespace gnss { -MappingUtils::MappingUtils() {} +MappingUtils::MappingUtils() +{ +} -MappingUtils::~MappingUtils() {} +MappingUtils::~MappingUtils() +{ +} -void MappingUtils::llaToxyz(const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out) { - if (params.proj_str.size() < 8) return; +void MappingUtils::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); + 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; - } + 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; + 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); + proj_destroy(P); + proj_context_destroy(C); } } // namespace gnss } // namespace interface @@ -58,7 +64,8 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms // Create MappingParameters struct to hold the parameters interface::gnss::MappingParameters params; - 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.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; From c73e5a304832b8753aa6d23df0c77c460300cdef Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 19 Mar 2024 18:24:50 +0900 Subject: [PATCH 08/12] fix build issues Signed-off-by: Maxime CLEMENT --- .../carla_autoware/resource/carla_autoware | 0 .../carla_pointcloud_interface/CMakeLists.txt | 49 +------------------ .../carla_pointcloud_interface/package.xml | 14 +----- 3 files changed, 2 insertions(+), 61 deletions(-) create mode 100644 simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware diff --git a/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware b/simulator/CARLA_Autoware/carla_autoware/resource/carla_autoware new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt index 692423f6d8aa0..832614f41a24f 100644 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt @@ -1,50 +1,12 @@ cmake_minimum_required(VERSION 3.5) +project(carla_pointcloud_interface) find_package(autoware_cmake REQUIRED) autoware_package() -project(carla_pointcloud_interface) -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) -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -find_path(YAML_CPP_INCLUDE_DIR - NAMES yaml_cpp.h - PATHS ${YAML_CPP_INCLUDE_DIRS}) -find_library(YAML_CPP_LIBRARY - NAMES YAML_CPP - PATHS ${YAML_CPP_LIBRARY_DIRS}) - -find_package(OpenCV REQUIRED) - -link_directories(${YAML_CPP_LIBRARY_DIRS}) - -if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - include_directories( - ${OpenCV_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ) @@ -52,20 +14,11 @@ 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/package.xml b/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml index 5272a666d9ee0..efca9e80b54f6 100644 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml @@ -7,27 +7,15 @@ Muhammad Raditya Giovanni Apache License 2.0 - ament_cmake_auto + autoware_cmake - angles - autoware_auto_control_msgs - autoware_auto_vehicle_msgs - cv_bridge - diagnostic_updater - geometry_msgs - image_transport libpcl-all-dev - message_filters - pcl_conversions pcl_ros rclcpp rclcpp_components sensor_msgs tf2 tf2_ros - velodyne_msgs - visualization_msgs - yaml-cpp ament_cmake From 343859c69087711fa519c1019766bcea22d73a2c Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 22 Mar 2024 12:03:19 +0900 Subject: [PATCH 09/12] cleaning some issues Signed-off-by: mraditya01 --- .../carla_autoware/config/objects.json | 10 ----- .../CARLA_Autoware/carla_autoware/package.xml | 7 ++-- .../CARLA_Autoware/carla_autoware/setup.py | 2 +- .../src/carla_autoware/carla_autoware.py | 13 ++----- .../carla_gnss_interface_node.hpp | 19 +-------- .../carla_gnss_interface_node.cpp | 13 +------ .../carla_pointcloud_interface/CMakeLists.txt | 39 ++++++++++++++++++- .../carla_pointcloud_interface_node.hpp | 1 - .../carla_pointcloud_interface_node.cpp | 4 -- 9 files changed, 50 insertions(+), 58 deletions(-) diff --git a/simulator/CARLA_Autoware/carla_autoware/config/objects.json b/simulator/CARLA_Autoware/carla_autoware/config/objects.json index ac6b39666ec5f..914a373af56d1 100644 --- a/simulator/CARLA_Autoware/carla_autoware/config/objects.json +++ b/simulator/CARLA_Autoware/carla_autoware/config/objects.json @@ -16,10 +16,6 @@ "type": "sensor.pseudo.markers", "id": "markers" }, - { - "type": "sensor.pseudo.opendrive_map", - "id": "map" - }, { "type": "vehicle.toyota.prius", "id": "ego_vehicle", @@ -58,12 +54,6 @@ "rotation_frequency": 20, "noise_stddev": 0.0 }, - { - "type": "sensor.opendrive_map", - "id": "OpenDRIVE", - "spawn_point": { "x": 0.0, "y": 0.0, "z": 1.6, "roll": 0.0, "pitch": 0.0 } - }, - { "type": "sensor.other.gnss", "id": "gnss", diff --git a/simulator/CARLA_Autoware/carla_autoware/package.xml b/simulator/CARLA_Autoware/carla_autoware/package.xml index 4cad00c7420fa..0ffc706488dd5 100644 --- a/simulator/CARLA_Autoware/carla_autoware/package.xml +++ b/simulator/CARLA_Autoware/carla_autoware/package.xml @@ -2,9 +2,10 @@ carla_autoware 0.0.0 - The carla_manual_control package - CARLA Simulator Team - MIT + The carla autoware bridge package + Muhammad Raditya Giovanni + Maxime CLEMENT + Apache License 2.0 ros2launch ros_compatibility diff --git a/simulator/CARLA_Autoware/carla_autoware/setup.py b/simulator/CARLA_Autoware/carla_autoware/setup.py index 139f98586afc6..75676e9485abb 100644 --- a/simulator/CARLA_Autoware/carla_autoware/setup.py +++ b/simulator/CARLA_Autoware/carla_autoware/setup.py @@ -32,7 +32,7 @@ maintainer="mradityagio", maintainer_email="mradityagio@gmail.com", description="CARLA ROS2 bridge for AUTOWARE", - license="MIT", + license="Apache License 2.0", tests_require=["pytest"], entry_points={ "console_scripts": ["carla_autoware = carla_autoware.carla_autoware:main"], diff --git a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py index 6b00e37c67e86..848dc6b4aceda 100644 --- a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py @@ -17,8 +17,6 @@ class CarlaVehicleInterface(Node): def __init__(self): - rclpy.init(args=None) - super().__init__("carla_vehicle_interface_node") client = carla.Client("localhost", 2000) @@ -29,11 +27,7 @@ def __init__(self): self.target_vel = 0.0 self.vel_diff = 0.0 self.current_control = carla.VehicleControl() - self.ros2_node = rclpy.create_node("carla_autoware") - - self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,)) - self.spin_thread.start() - + # Publishes Topics used for AUTOWARE self.pub_vel_state = self.ros2_node.create_publisher( VelocityReport, "/vehicle/status/velocity_status", 1 @@ -142,8 +136,9 @@ def publish_gnss(self, msg): def main(args=None): - CarlaVehicleInterface() - + rclpy.init() + node = CarlaVehicleInterface() + rclpy.spin(node) if __name__ == "__main__": main() 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 index 47e6d3821e1cc..2c6eaf56c6709 100644 --- 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 @@ -32,7 +32,6 @@ class GnssInterface : public rclcpp::Node { public: explicit GnssInterface(const rclcpp::NodeOptions & node_options); - virtual ~GnssInterface(); std::string tf_output_frame_; private: @@ -74,15 +73,6 @@ class GPSPoint alt = 0; dir = 0; } - - std::string ToString() - { - std::stringstream str; - str.precision(12); - str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl; - str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl; - return str.str(); - } }; class WayPoint @@ -109,15 +99,8 @@ struct MappingParameters double alt; }; -class MappingUtils -{ -public: - MappingUtils(); - virtual ~MappingUtils(); - static void llaToxyz( - const MappingParameters & params, double & x_out, double & y_out, double & z_out); -}; +void llaToxyz(const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out); } // namespace gnss } // namespace interface 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 index 468498b0d294a..6ba444f47d751 100644 --- 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 @@ -25,14 +25,8 @@ MappingUtils::MappingUtils() { } -MappingUtils::~MappingUtils() -{ -} - -void MappingUtils::llaToxyz( - const MappingParameters & params, double & x_out, double & y_out, double & z_out) -{ - if (params.proj_str.size() < 8) return; +void MappingUtils::llaToxyz(const MappingParameters ¶ms, 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); @@ -86,9 +80,6 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms pup_pose_with_cov->publish(pose_cov_); } -GnssInterface::~GnssInterface() -{ -} GnssInterface::GnssInterface(const rclcpp::NodeOptions & node_options) : Node("gnss_interface_node", node_options), tf_output_frame_("base_link") diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt index 832614f41a24f..7a1d863761fb0 100644 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt @@ -1,11 +1,39 @@ cmake_minimum_required(VERSION 3.5) -project(carla_pointcloud_interface) find_package(autoware_cmake REQUIRED) autoware_package() +project(carla_pointcloud_interface) +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) + +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + include_directories( ${PCL_COMMON_INCLUDE_DIRS} ) @@ -14,11 +42,20 @@ 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/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 index 1498321c1fe7a..cc6bc5950cf81 100644 --- 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 @@ -31,7 +31,6 @@ class PointCloudInterface : public rclcpp::Node { public: explicit PointCloudInterface(const rclcpp::NodeOptions & node_options); - virtual ~PointCloudInterface(); private: std::shared_ptr tf_buffer_; 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 index 187553da913eb..27b89d7054408 100644 --- 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 @@ -35,10 +35,6 @@ void PointCloudInterface::setupTF() tf_listener_ = std::make_shared(*tf_buffer_); } -PointCloudInterface::~PointCloudInterface() -{ -} - PointCloudInterface::PointCloudInterface(const rclcpp::NodeOptions & node_options) : Node("carla_pointcloud_interface_node", node_options), tf_output("base_link") { From 6e22579d873aeb1a883e121cdfb810f49165dbdf Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 22 Mar 2024 12:06:13 +0900 Subject: [PATCH 10/12] cleaning some issues Signed-off-by: mraditya01 --- .../carla_autoware/CMakeLists.txt | 33 --------------- .../CARLA_Autoware/carla_autoware/README.md | 16 ++++---- .../CARLA_Autoware/carla_autoware/package.xml | 2 - .../src/carla_autoware/carla_autoware.py | 41 +++++++------------ .../carla_gnss_interface_node.hpp | 3 +- .../carla_gnss_interface_node.cpp | 15 +++---- .../carla_pointcloud_interface/CMakeLists.txt | 8 +--- 7 files changed, 28 insertions(+), 90 deletions(-) diff --git a/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt b/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt index c51fe86d73706..f4471accbffd0 100644 --- a/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_autoware/CMakeLists.txt @@ -8,39 +8,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - - -# find dependencies -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(sensor_msgs_py REQUIRED) -find_package(datetime REQUIRED) - -find_package(rclpy REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(carla_msgs REQUIRED) -find_package(autoware_auto_vehicle_msgs REQUIRED) -find_package(autoware_auto_control_msgs REQUIRED) -find_package(tier4_debug_msgs REQUIRED) -find_package(tier4_system_msgs REQUIRED) -find_package(autoware_adapi_v1_msgs REQUIRED) -find_package(transform3d REQUIRED) -find_package(math REQUIRED) -find_package(numpy REQUIRED) -find_package(carla_data_provider REQUIRED) - - - - -find_package(ros_environment REQUIRED) -set(ROS_VERSION $ENV{ROS_VERSION}) - ament_export_dependencies(rclpy) # Install launch files. diff --git a/simulator/CARLA_Autoware/carla_autoware/README.md b/simulator/CARLA_Autoware/carla_autoware/README.md index cbf6f4e75f2b0..e77a9f920f6e2 100644 --- a/simulator/CARLA_Autoware/carla_autoware/README.md +++ b/simulator/CARLA_Autoware/carla_autoware/README.md @@ -6,15 +6,15 @@ 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.14-ubuntu-22.04). -- Add the egg file to the folder: ../CARLA_0.9.14/PythonAPI/carla/dist +- 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. # Environment | ubuntu | ros | carla | autoware | | :----: | :----: | :----: | :-------------: | -| 22.04 | humble | 0.9.14 | universe/master | +| 22.04 | humble | 0.9.15 | universe/master | # Setup @@ -26,15 +26,13 @@ This ros package enables autonomous driving using Autoware in addition to the ba 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 and ROSBridge +- Clone this repositories for ros-bridge on ROS2 Humble. ``` - git clone https://github.com/mraditya01/CARLA_Autoware.git 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 ``` -- Copy the files (sensor_kit_calibration.yaml, sensors.calibration.yaml) from folder "GNSS_interface/src/carla_sensor_kit_launch/carla_sensor_kit_description/config" to "src/param/autoware_individual_params/carla_sensor_kit". - ## build ```bash @@ -48,13 +46,13 @@ colcon build --symlink-install ```bash cd CARLA -./CarlaUE4.sh -prefernvidia -quality-level=Low +./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 +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 ``` 3. Set initial pose (Init by GNSS) diff --git a/simulator/CARLA_Autoware/carla_autoware/package.xml b/simulator/CARLA_Autoware/carla_autoware/package.xml index 0ffc706488dd5..859139d93b1ce 100644 --- a/simulator/CARLA_Autoware/carla_autoware/package.xml +++ b/simulator/CARLA_Autoware/carla_autoware/package.xml @@ -13,7 +13,6 @@ astuff_sensor_msgs autoware_auto_control_msgs autoware_auto_vehicle_msgs - carla_data_provider carla_msgs datetime geometry_msgs @@ -25,7 +24,6 @@ sensor_msgs_py tf2 tf2_ros - transform3d ament_cmake diff --git a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py index 848dc6b4aceda..425dd4ea66221 100644 --- a/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py +++ b/simulator/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py @@ -1,5 +1,3 @@ -import threading - from autoware_auto_control_msgs.msg import AckermannControlCommand from autoware_auto_vehicle_msgs.msg import ControlModeReport from autoware_auto_vehicle_msgs.msg import GearReport @@ -20,51 +18,43 @@ def __init__(self): super().__init__("carla_vehicle_interface_node") client = carla.Client("localhost", 2000) - client.set_timeout(20) + 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.ros2_node.create_publisher( + self.pub_vel_state = self.create_publisher( VelocityReport, "/vehicle/status/velocity_status", 1 ) - self.pub_steering_state = self.ros2_node.create_publisher( + self.pub_steering_state = self.create_publisher( SteeringReport, "/vehicle/status/steering_status", 1 ) - self.pub_ctrl_mode = self.ros2_node.create_publisher( + self.pub_ctrl_mode = self.create_publisher( ControlModeReport, "/vehicle/status/control_mode", 1 ) - self.pub_gear_state = self.ros2_node.create_publisher( - GearReport, "/vehicle/status/gear_status", 1 - ) - self.pub_control = self.ros2_node.create_publisher( + 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.vehicle_imu_publisher = self.ros2_node.create_publisher( - Imu, "/sensing/imu/tamagawa/imu_raw", 1 - ) - self.sensing_cloud_publisher = self.ros2_node.create_publisher( - PointCloud2, "/carla_pointcloud", 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) # Subscribe Topics used in Control - self.sub_status = self.ros2_node.create_subscription( + self.sub_status = self.create_subscription( CarlaEgoVehicleStatus, "/carla/ego_vehicle/vehicle_status", self.ego_status_callback, 1 ) - self.sub_control = self.ros2_node.create_subscription( + self.sub_control = self.create_subscription( AckermannControlCommand, "/control/command/control_cmd", self.control_callback, qos_profile=QoSProfile(depth=1), ) - self.sub_imu = self.ros2_node.create_subscription( - Imu, "/carla/ego_vehicle/imu", self.publish_imu, 1 - ) - self.sub_lidar = self.ros2_node.create_subscription( + 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, @@ -130,15 +120,12 @@ def publish_imu(self, imu_msg): imu_msg.header.frame_id = "tamagawa/imu_link" self.vehicle_imu_publisher.publish(imu_msg) - def publish_gnss(self, msg): - """Publish GNSS to Autoware.""" - self.publisher_map.publish(msg) - def main(args=None): rclpy.init() node = CarlaVehicleInterface() rclpy.spin(node) + if __name__ == "__main__": main() 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 index 2c6eaf56c6709..a3a237fd5935c 100644 --- 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 @@ -99,8 +99,7 @@ struct MappingParameters double alt; }; - -void llaToxyz(const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out); +void llaToxyz(const MappingParameters & params, double & x_out, double & y_out, double & z_out); } // namespace gnss } // namespace interface 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 index 6ba444f47d751..a12da2b13b014 100644 --- 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 @@ -21,12 +21,9 @@ namespace interface namespace gnss { -MappingUtils::MappingUtils() +void llaToxyz(const MappingParameters & params, double & x_out, double & y_out, double & z_out) { -} - -void MappingUtils::llaToxyz(const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out) { - if (params.proj_str.size() < 8) return; + 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); @@ -45,6 +42,7 @@ void MappingUtils::llaToxyz(const MappingParameters ¶ms, double &x_out, doub proj_destroy(P); proj_context_destroy(C); } + } // namespace gnss } // namespace interface @@ -53,11 +51,9 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms geometry_msgs::msg::PoseStamped pose_; geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_; interface::gnss::WayPoint origin, p; - // Create an instance of MappingUtils - interface::gnss::MappingUtils mappingUtils; + interface::gnss::MappingParameters params; // Create MappingParameters struct to hold the parameters - interface::gnss::MappingParameters params; 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; @@ -65,7 +61,7 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms params.alt = msg->altitude; // Call llaToxyz function - mappingUtils.llaToxyz(params, p.pos.x, p.pos.y, p.pos.z); + interface::gnss::llaToxyz(params, p.pos.x, p.pos.y, p.pos.z); pose_.header = msg->header; pose_.header.frame_id = "map"; @@ -80,7 +76,6 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms 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") { diff --git a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt index 7a1d863761fb0..66f98f9efabd5 100644 --- a/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt +++ b/simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) +project(carla_pointcloud_interface) find_package(autoware_cmake REQUIRED) autoware_package() -project(carla_pointcloud_interface) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() @@ -28,12 +28,6 @@ find_package(PCL REQUIRED COMPONENTS common) find_package(PkgConfig REQUIRED) -link_directories(${YAML_CPP_LIBRARY_DIRS}) - -if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - include_directories( ${PCL_COMMON_INCLUDE_DIRS} ) From fc751aff603870aa4b89020010c8882c1bd686ca Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Fri, 12 Apr 2024 18:20:21 +0900 Subject: [PATCH 11/12] fixed performance and rearrange some packages --- .pre-commit-config.yaml | 6 - .../carla_autoware/CMakeLists.txt | 0 .../carla_autoware/README.md | 15 +-- .../carla_autoware/config/objects.json | 37 ++---- .../launch/carla_autoware.launch.xml | 6 + .../launch/carla_ros.launch.xml | 16 +-- .../carla_autoware/package.xml | 1 + .../carla_autoware/resource/carla_autoware | 0 .../carla_autoware/setup.cfg | 0 .../carla_autoware/setup.py | 0 .../src/carla_autoware/carla_autoware.py | 91 +++++++++------ .../launch/carla_autoware.launch.xml | 13 --- .../carla_gnss_interface/CMakeLists.txt | 41 ------- .../carla_gnss_interface/README.md | 7 -- .../carla_gnss_interface_node.hpp | 107 ------------------ .../launch/gnss_interface.launch.xml | 5 - .../carla_gnss_interface/package.xml | 29 ----- .../carla_gnss_interface_node.cpp | 92 --------------- .../carla_pointcloud_interface/CMakeLists.txt | 56 --------- .../carla_pointcloud_interface/README.md | 11 -- .../carla_pointcloud_interface_node.hpp | 45 -------- .../launch/carla_pointcloud_interface.xml | 3 - .../carla_pointcloud_interface/package.xml | 23 ---- .../carla_pointcloud_interface_node.cpp | 52 --------- 24 files changed, 89 insertions(+), 567 deletions(-) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/CMakeLists.txt (100%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/README.md (73%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/config/objects.json (71%) create mode 100644 simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/launch/carla_ros.launch.xml (79%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/package.xml (96%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/resource/carla_autoware (100%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/setup.cfg (100%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/setup.py (100%) rename simulator/CARLA_Autoware/{ => CARLA_Autoware}/carla_autoware/src/carla_autoware/carla_autoware.py (67%) delete mode 100644 simulator/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/CMakeLists.txt delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/README.md delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/include/carla_gnss_interface/carla_gnss_interface_node.hpp delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/launch/gnss_interface.launch.xml delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/package.xml delete mode 100644 simulator/CARLA_Autoware/carla_gnss_interface/src/carla_gnss_interface/carla_gnss_interface_node.cpp delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/CMakeLists.txt delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/README.md delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/include/carla_pointcloud_interface/carla_pointcloud_interface_node.hpp delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/launch/carla_pointcloud_interface.xml delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/package.xml delete mode 100644 simulator/CARLA_Autoware/carla_pointcloud_interface/src/carla_pointcloud_interface/carla_pointcloud_interface_node.cpp 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) From 9d5a7185be3ff8e04691ff857e17cda0032f140b Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Mon, 15 Apr 2024 15:03:04 +0900 Subject: [PATCH 12/12] fixed package names --- .../CARLA_Autoware => }/carla_autoware/CMakeLists.txt | 0 .../CARLA_Autoware => }/carla_autoware/README.md | 2 +- .../CARLA_Autoware => }/carla_autoware/config/objects.json | 0 .../carla_autoware/launch/carla_autoware.launch.xml | 0 .../carla_autoware/launch/carla_ros.launch.xml | 0 .../CARLA_Autoware => }/carla_autoware/package.xml | 0 .../carla_autoware/resource/carla_autoware | 0 .../CARLA_Autoware => }/carla_autoware/setup.cfg | 0 .../CARLA_Autoware => }/carla_autoware/setup.py | 5 +++-- .../carla_autoware/src/carla_autoware/carla_autoware.py | 0 10 files changed, 4 insertions(+), 3 deletions(-) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/CMakeLists.txt (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/README.md (96%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/config/objects.json (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/launch/carla_autoware.launch.xml (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/launch/carla_ros.launch.xml (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/package.xml (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/resource/carla_autoware (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/setup.cfg (100%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/setup.py (81%) rename simulator/{CARLA_Autoware/CARLA_Autoware => }/carla_autoware/src/carla_autoware/carla_autoware.py (100%) diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/CMakeLists.txt b/simulator/carla_autoware/CMakeLists.txt similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/CMakeLists.txt rename to simulator/carla_autoware/CMakeLists.txt diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/README.md b/simulator/carla_autoware/README.md similarity index 96% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/README.md rename to simulator/carla_autoware/README.md index 8e5abd137c67c..6691bc6517410 100644 --- a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/README.md +++ b/simulator/carla_autoware/README.md @@ -21,7 +21,7 @@ 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/) -- [Carla Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) +- [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 diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/config/objects.json b/simulator/carla_autoware/config/objects.json similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/config/objects.json rename to simulator/carla_autoware/config/objects.json diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml b/simulator/carla_autoware/launch/carla_autoware.launch.xml similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_autoware.launch.xml rename to simulator/carla_autoware/launch/carla_autoware.launch.xml diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml b/simulator/carla_autoware/launch/carla_ros.launch.xml similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/launch/carla_ros.launch.xml rename to simulator/carla_autoware/launch/carla_ros.launch.xml diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/package.xml b/simulator/carla_autoware/package.xml similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/package.xml rename to simulator/carla_autoware/package.xml diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/resource/carla_autoware b/simulator/carla_autoware/resource/carla_autoware similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/resource/carla_autoware rename to simulator/carla_autoware/resource/carla_autoware diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.cfg b/simulator/carla_autoware/setup.cfg similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.cfg rename to simulator/carla_autoware/setup.cfg diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.py b/simulator/carla_autoware/setup.py similarity index 81% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.py rename to simulator/carla_autoware/setup.py index 75676e9485abb..82dfb6581d776 100644 --- a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/setup.py +++ b/simulator/carla_autoware/setup.py @@ -23,9 +23,10 @@ packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, glob("config/*.json")), + ("share/" + package_name, glob("config/objects.json")), ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name), glob("launch/*.launch.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, diff --git a/simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py b/simulator/carla_autoware/src/carla_autoware/carla_autoware.py similarity index 100% rename from simulator/CARLA_Autoware/CARLA_Autoware/carla_autoware/src/carla_autoware/carla_autoware.py rename to simulator/carla_autoware/src/carla_autoware/carla_autoware.py