feat: to add lidar simulation, create the simple lidar simulator node…

… and launch file (#41) * feat: to add lidar simulation, create the simple lidar simulator node and launch file * fix: to pass the build, modify the code * feat: to handle multiple objects, manage them using a list * feat: o make object positions easily changeable, load positions from CSV file * refactor: to improve readability, modify the callback function * refactor: to ensure consistency in notation, modify "read" to " load" * feat: make lidar moving with the vehicle * feat: add the sensing and perception nodes Signed-off-by: Masahiro Kubota <[email protected]> * fix: to remove pointcloud in section 1 and 2, clear contents of object_centers.csv Signed-off-by: Masahiro Kubota <[email protected]> * fix: modify the comment Signed-off-by: Masahiro Kubota <[email protected]> --------- Signed-off-by: Masahiro Kubota <[email protected]>
AutomotiveAIChallenge · Jun 15, 2024 · 30104f1 · 30104f1
1 parent f964156
commit 30104f1
Show file tree

Hide file tree

Showing 9 changed files with 265 additions and 0 deletions.
diff --git a/src/autoware_practice_driver/launch/vehicle.launch.xml b/src/autoware_practice_driver/launch/vehicle.launch.xml
@@ -21,4 +21,12 @@
   </node>
   <node pkg="autoware_practice_dummy_localizer" exec="dummy_localizer" name="dummy_localizer">
   </node>
+  <node pkg="topic_tools" exec="relay" name="lidar_driver">
+    <param name="input_topic" value="/simulator/ground_truth/lidar0/pointcloud"/>
+    <param name="output_topic" value="/sensing/lidar/concatenated/pointcloud"/>
+  </node>
+  <node pkg="topic_tools" exec="relay" name="object_segmentation">
+    <param name="input_topic" value="/sensing/lidar/concatenated/pointcloud"/>
+    <param name="output_topic" value="/perception/obstacle_segmentation/pointcloud"/>
+  </node>
 </launch>
diff --git 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
@@ -7,4 +7,5 @@
   <include file="$(find-pkg-share autoware_practice_driver)/launch/vehicle.launch.xml"/>
   <include file="$(find-pkg-share autoware_practice_simulator)/launch/simulator.launch.xml"/>
   <include file="$(find-pkg-share autoware_practice_evaluator)/launch/evaluator.launch.xml"/>
+  <include file="$(find-pkg-share autoware_practice_lidar_simulator)/launch/lidar_simulator.launch.xml"/>
 </launch>
diff --git a/src/autoware_practice_lidar_simulator/CMakeLists.txt 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
diff --git a/src/autoware_practice_lidar_simulator/launch/lidar_simulator.launch.xml b/src/autoware_practice_lidar_simulator/launch/lidar_simulator.launch.xml
@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="autoware_practice_lidar_simulator" exec="simple_lidar_simulator" name="simple_lidar_simulator">
+  </node>
+</launch>
diff --git a/src/autoware_practice_lidar_simulator/package.xml b/src/autoware_practice_lidar_simulator/package.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_practice_lidar_simulator</name>
+  <version>0.0.0</version>
+  <description>The autoware_practice_course package</description>
+  <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>pcl_conversions</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp b/src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp
@@ -0,0 +1,132 @@
+#include "simple_lidar_simulator.hpp"
+#include <pcl_conversions/pcl_conversions.h>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <string>
+
+namespace autoware_practice_lidar_simulator
+{
+
+SampleNode::SampleNode() : Node("simple_lidar_simulator")
+{
+    publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("/simulator/ground_truth/lidar0/pointcloud", 10);
+    pose_subscriber_ = create_subscription<PoseStamped>(
+        "/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<std::pair<float, float>> SampleNode::load_object_centers_from_csv(const std::string& file_path)
+{
+    std::vector<std::pair<float, float>> 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<PointCloudXYZ>();
+
+    // 各物体の点群を生成
+    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<pcl::PointXYZ>::Ptr SampleNode::create_object_point_cloud(float x_center, float y_center, float width, float height, float resolution)
+{
+    auto cloud = std::make_shared<PointCloudXYZ>();
+
+    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<pcl::PointXYZ>::Ptr SampleNode::filter_points_within_radius(PointCloudXYZ::Ptr cloud, float radius)
+{
+    auto filtered_cloud = std::make_shared<PointCloudXYZ>();
+    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
@@ -0,0 +1,41 @@
+#ifndef SIMPLE_LIDAR_SIMULATOR_HPP
+#define SIMPLE_LIDAR_SIMULATOR_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+
+
+namespace autoware_practice_lidar_simulator
+{
+class SampleNode : public rclcpp::Node
+{
+public:
+    SampleNode();
+
+private:
+    using PointCloud2 = sensor_msgs::msg::PointCloud2;
+    using PointCloudXYZ = pcl::PointCloud<pcl::PointXYZ>;
+    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<std::pair<float, float>> load_object_centers_from_csv(const std::string& file_path);
+    void pose_callback(const PoseStamped::SharedPtr msg);
+
+    rclcpp::Publisher<PointCloud2>::SharedPtr publisher_;
+    rclcpp::Subscription<PoseStamped>::SharedPtr pose_subscriber_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    std::vector<std::pair<float, float>> object_centers_;
+    Pose last_pose_;
+
+};
+
+}
+
+#endif  // SIMPLE_LIDAR_SIMULATOR_HPP