diff --git a/src/autoware_practice_driver/launch/vehicle.launch.xml b/src/autoware_practice_driver/launch/vehicle.launch.xml index 93b1775..38aeb44 100644 --- a/src/autoware_practice_driver/launch/vehicle.launch.xml +++ b/src/autoware_practice_driver/launch/vehicle.launch.xml @@ -21,4 +21,12 @@ + + + + + + + + diff --git a/src/autoware_practice_launch/config/autoware.rviz b/src/autoware_practice_launch/config/autoware.rviz index 3878410..e17f9a9 100644 --- a/src/autoware_practice_launch/config/autoware.rviz +++ b/src/autoware_practice_launch/config/autoware.rviz @@ -71,7 +71,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /evaluator/markers Value: true + - Class: rviz_default_plugins/PointCloud2 + Enabled: true + Name: PointCloud + Topic: + Depth: 1 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/concatenated/pointcloud + Size (m): 0.1 + Value: true Enabled: true + Global Options: Background Color: 48; 48; 48 Fixed Frame: map diff --git a/src/autoware_practice_launch/launch/practice.launch.xml b/src/autoware_practice_launch/launch/practice.launch.xml index 7c6cb0f..475c900 100644 --- a/src/autoware_practice_launch/launch/practice.launch.xml +++ b/src/autoware_practice_launch/launch/practice.launch.xml @@ -7,4 +7,5 @@ + diff --git a/src/autoware_practice_lidar_simulator/CMakeLists.txt b/src/autoware_practice_lidar_simulator/CMakeLists.txt new file mode 100644 index 0000000..5b5807f --- /dev/null +++ b/src/autoware_practice_lidar_simulator/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_practice_lidar_simulator) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(PCL REQUIRED COMPONENTS common io) + +include_directories( + include + ${PCL_INCLUDE_DIRS} +) + + +ament_auto_add_executable(simple_lidar_simulator src/simple_lidar_simulator.cpp) + +target_link_libraries(simple_lidar_simulator ${PCL_LIBRARIES}) + +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/src/autoware_practice_lidar_simulator/config/object_centers.csv b/src/autoware_practice_lidar_simulator/config/object_centers.csv new file mode 100644 index 0000000..e69de29 diff --git a/src/autoware_practice_lidar_simulator/launch/lidar_simulator.launch.xml b/src/autoware_practice_lidar_simulator/launch/lidar_simulator.launch.xml new file mode 100644 index 0000000..21f32e0 --- /dev/null +++ b/src/autoware_practice_lidar_simulator/launch/lidar_simulator.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/autoware_practice_lidar_simulator/package.xml b/src/autoware_practice_lidar_simulator/package.xml new file mode 100644 index 0000000..ffe8e15 --- /dev/null +++ b/src/autoware_practice_lidar_simulator/package.xml @@ -0,0 +1,33 @@ + + + + autoware_practice_lidar_simulator + 0.0.0 + The autoware_practice_course package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_auto_planning_msgs + geometry_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + pcl_conversions + sensor_msgs + + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_lint_cmake + ament_cmake_xmllint + ament_lint_auto + + + ament_cmake + + diff --git a/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp b/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp new file mode 100644 index 0000000..2ffd045 --- /dev/null +++ b/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp @@ -0,0 +1,132 @@ +#include "simple_lidar_simulator.hpp" +#include +#include +#include +#include +#include + +namespace autoware_practice_lidar_simulator +{ + +SampleNode::SampleNode() : Node("simple_lidar_simulator") +{ + publisher_ = create_publisher("/simulator/ground_truth/lidar0/pointcloud", 10); + pose_subscriber_ = create_subscription( + "/simulator/ground_truth/pose", 10, std::bind(&SampleNode::pose_callback, this, std::placeholders::_1)); + const auto period = rclcpp::Rate(10).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); + + + // 物体の中心位置リストをCSVから読み取って初期化 + object_centers_ = load_object_centers_from_csv("src/autoware_practice_lidar_simulator/config/object_centers.csv"); +} + +void SampleNode::pose_callback(const PoseStamped::SharedPtr msg) +{ + last_pose_ = msg->pose; +} +std::vector> SampleNode::load_object_centers_from_csv(const std::string& file_path) +{ + std::vector> centers; + std::ifstream file(file_path); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", file_path.c_str()); + return centers; + } + + std::string line; + std::getline(file, line); + RCLCPP_INFO(this->get_logger(), "Skipping header: %s", line.c_str()); + + while (std::getline(file, line)) + { + RCLCPP_INFO(this->get_logger(), "Processing line: %s", line.c_str()); + std::istringstream line_stream(line); + std::string x_str, y_str; + + if (std::getline(line_stream, x_str, ',') && std::getline(line_stream, y_str, ',')) + { + float x = std::stof(x_str); + float y = std::stof(y_str); + centers.emplace_back(x, y); + } + } + file.close(); + + return centers; +} + + +void SampleNode::on_timer() +{ + auto cloud = std::make_shared(); + + // 各物体の点群を生成 + for (const auto& center : object_centers_) + { + auto object_cloud = create_object_point_cloud(center.first, center.second, 3.0, 2.0, 0.5); + *cloud += *object_cloud; + } + // 車両から半径10m以内の点群を抽出 + auto filtered_cloud = filter_points_within_radius(cloud, 10.0); + + sensor_msgs::msg::PointCloud2 output; + pcl::toROSMsg(*filtered_cloud, output); + output.header.frame_id = "map"; + output.header.stamp = this->now(); + + publisher_->publish(output); +} + +pcl::PointCloud::Ptr SampleNode::create_object_point_cloud(float x_center, float y_center, float width, float height, float resolution) +{ + auto cloud = std::make_shared(); + + float x_start = x_center - width / 2; + float x_end = x_center + width / 2; + float y_start = y_center - height / 2; + float y_end = y_center + height / 2; + + // 上辺と下辺 + for (float x = x_start; x <= x_end; x += resolution) + { + cloud->points.emplace_back(pcl::PointXYZ{x, y_start, 0.0}); + cloud->points.emplace_back(pcl::PointXYZ{x, y_end, 0.0}); + } + + // 左辺と右辺 + for (float y = y_start; y <= y_end; y += resolution) + { + cloud->points.emplace_back(pcl::PointXYZ{x_start, y, 0.0}); + cloud->points.emplace_back(pcl::PointXYZ{x_end, y, 0.0}); + } + + return cloud; +} + +pcl::PointCloud::Ptr SampleNode::filter_points_within_radius(PointCloudXYZ::Ptr cloud, float radius) +{ + auto filtered_cloud = std::make_shared(); + for (const auto& point : cloud->points) + { + float x = point.x - last_pose_.position.x; + float y = point.y - last_pose_.position.y; + if (sqrt(x * x + y * y) <= radius) + { + filtered_cloud->points.push_back(point); + } + } + return filtered_cloud; +} + +} + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared< autoware_practice_lidar_simulator::SampleNode>(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.hpp b/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.hpp new file mode 100644 index 0000000..03d2f32 --- /dev/null +++ b/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.hpp @@ -0,0 +1,41 @@ +#ifndef SIMPLE_LIDAR_SIMULATOR_HPP +#define SIMPLE_LIDAR_SIMULATOR_HPP + +#include +#include +#include +#include +#include + + + +namespace autoware_practice_lidar_simulator +{ +class SampleNode : public rclcpp::Node +{ +public: + SampleNode(); + +private: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloudXYZ = pcl::PointCloud; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Pose = geometry_msgs::msg::Pose; + + void on_timer(); + PointCloudXYZ::Ptr create_object_point_cloud(float x_center, float y_center, float width, float height, float resolution); + PointCloudXYZ::Ptr filter_points_within_radius(PointCloudXYZ::Ptr cloud, float radius); + std::vector> load_object_centers_from_csv(const std::string& file_path); + void pose_callback(const PoseStamped::SharedPtr msg); + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr pose_subscriber_; + rclcpp::TimerBase::SharedPtr timer_; + std::vector> object_centers_; + Pose last_pose_; + +}; + +} + +#endif // SIMPLE_LIDAR_SIMULATOR_HPP