feat: modify the code to avoide tobjects

Signed-off-by: Masahiro Kubota <[email protected]>

src/autoware_practice_course/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 find_package(PCL REQUIRED COMPONENTS common io)
+find_package(visualization_msgs REQUIRED)  # 追加
 
 include_directories(
   include
@@ -27,8 +28,10 @@ ament_auto_add_executable(longitudinal_controller src/velocity_planning/longitud
 ament_auto_add_executable(trajectory_loader src/velocity_planning/trajectory_loader.cpp)
 ament_auto_add_executable(trajectory_follower src/velocity_planning/trajectory_follower.cpp)
 ament_auto_add_executable(trajectory_planner src/avoidance/trajectory_planner.cpp)
+ament_auto_add_executable(trajectory_visualizer src/avoidance/marker.cpp)
 
 target_link_libraries(trajectory_planner ${PCL_LIBRARIES})
+target_link_libraries(trajectory_visualizer ${visualization_msgs_LIBRARIES})  # 追加
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/autoware_practice_course/package.xml

@@ -20,6 +20,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_copyright</test_depend>
   <test_depend>ament_cmake_cppcheck</test_depend>

src/autoware_practice_course/src/avoidance/marker.cpp

@@ -0,0 +1,113 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 #include <memory>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 <rclcpp/rclcpp.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+class TrajectoryVisualizer : public rclcpp::Node
+{
+public:
+  explicit TrajectoryVisualizer(const rclcpp::NodeOptions & options) : Node("trajectory_visualizer", options)
+  {
+    trajectory_sub_ = this->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
+      "/planning/scenario_planning/trajectory", 10,
+      std::bind(&TrajectoryVisualizer::trajectoryCallback, this, std::placeholders::_1));
+    reference_trajectory_sub_ = this->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
+      "/planning/trajectory_loader/trajectory", 10,
+      std::bind(&TrajectoryVisualizer::reference_trajectoryCallback, this, std::placeholders::_1));
+
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/trajectory_marker", 10);
+    reference_marker_pub_ =
+      this->create_publisher<visualization_msgs::msg::MarkerArray>("/reference_trajectory_marker", 10);
+  }
+
+private:
+  void trajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg)
+  {
+    visualization_msgs::msg::MarkerArray marker_array;
+
+    for (size_t i = 0; i < msg->points.size(); ++i) {
+      visualization_msgs::msg::Marker marker;
+      marker.header = msg->header;
+      marker.header.frame_id = "map";  // 適切なフレームIDを設定
+      marker.ns = "trajectory";
+      marker.id = i;
+      marker.type = visualization_msgs::msg::Marker::SPHERE;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+
+      marker.pose.position = msg->points[i].pose.position;
+      marker.pose.orientation.w = 1.0;
+
+      marker.scale.x = 0.2;
+      marker.scale.y = 0.2;
+      marker.scale.z = 0.2;
+
+      marker.color.r = 1.0;
+      marker.color.g = 0.0;
+      marker.color.b = 0.0;
+      marker.color.a = 1.0;
+
+      marker_array.markers.push_back(marker);
+    }
+
+    marker_pub_->publish(marker_array);
+  }
+
+  void reference_trajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg)
+  {
+    visualization_msgs::msg::MarkerArray marker_array;
+
+    for (size_t i = 0; i < msg->points.size(); ++i) {
+      visualization_msgs::msg::Marker marker;
+      marker.header = msg->header;
+      marker.header.frame_id = "map";  // 適切なフレームIDを設定
+      marker.ns = "trajectory";
+      marker.id = i;
+      marker.type = visualization_msgs::msg::Marker::SPHERE;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+
+      marker.pose.position = msg->points[i].pose.position;
+      marker.pose.orientation.w = 1.0;
+
+      marker.scale.x = 0.2;
+      marker.scale.y = 0.2;
+      marker.scale.z = 0.2;
+
+      marker.color.r = 0.0;
+      marker.color.g = 1.0;
+      marker.color.b = 0.0;
+      marker.color.a = 1.0;
+
+      marker_array.markers.push_back(marker);
+    }
+
+    reference_marker_pub_->publish(marker_array);
+  }
+
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr trajectory_sub_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr reference_trajectory_sub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr reference_marker_pub_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto options = rclcpp::NodeOptions();
+  auto node = std::make_shared<TrajectoryVisualizer>(options);
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/autoware_practice_course/src/avoidance/trajectory_planner.cpp

@@ -27,7 +27,7 @@ namespace autoware_practice_course
 SampleNode::SampleNode() : Node("trajectory_planner")
 {
   using std::placeholders::_1;
-  GRID_RESOLUTION_ = 1.0;  // 1セルのサイズ（メートル）
+  GRID_RESOLUTION_ = 3;  // 1セルのサイズ（メートル）
   GRID_WIDTH_ = 100.0;
   GRID_HEIGHT_ = 100.0;
   state_num_ = 10;
@@ -64,10 +64,6 @@ void SampleNode::update_reference_trajectory(const Trajectory & msg)  // called
 
 void SampleNode::update_pointcloud(const PointCloud2 & msg)  // called by sub_pointcloud_
 {
-  if (msg.data.empty()) {
-    RCLCPP_ERROR(this->get_logger(), "Received empty pointcloud message");
-    return;
-  }
   pointcloud_ = msg;
   pointcloud_initialized_ = true;
   RCLCPP_INFO(this->get_logger(), "PointCloud received and updated.");
@@ -96,7 +92,13 @@ SampleNode::Trajectory SampleNode::create_trajectory()  // called by on_timer()
 
   // evaluate trahectories by the cost map
   Trajectory best_trajectory = evaluate_trajectory(trajectory_set, costmap);
-  RCLCPP_INFO(this->get_logger(), "Finished creating trajectory.");
+  RCLCPP_INFO(this->get_logger(), "Logging trajectory with %zu points", best_trajectory.points.size());
+  for (size_t i = 0; i < best_trajectory.points.size(); ++i) {
+    const auto & point = best_trajectory.points[i];
+    RCLCPP_INFO(
+      this->get_logger(), "Point %zu: x=%f, y=%f, z=%f", i, point.pose.position.x, point.pose.position.y,
+      point.pose.position.z);
+  }
 
   return best_trajectory;
 }
@@ -158,6 +160,7 @@ std::vector<SampleNode::Trajectory> SampleNode::create_trajectory_set()
     for (const auto & interpolated_point : interpolated_points) {
       TrajectoryPoint trajectory_point;
       trajectory_point.pose.position = interpolated_point;
+      trajectory_point.longitudinal_velocity_mps = 2.0;
       trajectory_candidate.points.push_back(trajectory_point);
 
       RCLCPP_INFO(
@@ -316,11 +319,9 @@ std::vector<std::vector<float>> SampleNode::create_costmap()
   // 点群をグリッドマップに変換
   for (const auto & point : pointcloud_pcl->points) {
     int x_index = static_cast<int>(point.x / GRID_RESOLUTION_);
-    int y_index = static_cast<int>(point.y / GRID_RESOLUTION_);
+    int y_index = static_cast<int>((point.y + GRID_RESOLUTION_ / 2) / GRID_RESOLUTION_);
 
-    if (
-      x_index >= -GRID_WIDTH_ / 2 && x_index < GRID_WIDTH_ / 2 && y_index >= -GRID_HEIGHT_ / 2 &&
-      y_index < GRID_HEIGHT_ / 2) {
+    if (x_index >= 0 && x_index < GRID_WIDTH_ && y_index >= 0 && y_index < GRID_HEIGHT_) {
       costmap[x_index][y_index] = 100.0;  // 点が存在する格子は評価関数を高く設定
     }
   }
@@ -362,12 +363,10 @@ SampleNode::Trajectory SampleNode::evaluate_trajectory(
     // trajectoryをcostmapで評価
     for (const auto & trajectory_point : trajectory_candidate.points) {
       int x_index = static_cast<int>(trajectory_point.pose.position.x / GRID_RESOLUTION_);
-      int y_index = static_cast<int>(trajectory_point.pose.position.y / GRID_RESOLUTION_);
+      int y_index = static_cast<int>((trajectory_point.pose.position.y + GRID_RESOLUTION_ / 2) / GRID_RESOLUTION_);
       RCLCPP_INFO(this->get_logger(), "x_index=%d, y_index=%d", x_index, y_index);
 
-      if (
-        x_index >= -GRID_WIDTH_ / 2 && x_index < GRID_WIDTH_ / 2 && y_index >= -GRID_HEIGHT_ / 2 &&
-        y_index < GRID_HEIGHT_ / 2) {
+      if (x_index >= 0 && x_index < GRID_WIDTH_ && y_index >= 0 && y_index < GRID_HEIGHT_) {
         trajectory_cost[index] += costmap[x_index][y_index];
       } else {
         RCLCPP_WARN(

src/autoware_practice_course/src/velocity_planning/trajectory_loader.cpp

@@ -25,7 +25,7 @@ namespace autoware_practice_course
 
 SampleNode::SampleNode() : Node("trajectory_loader")
 {
-  pub_trajectory_ = create_publisher<Trajectory>("/planning/scenario_planning/trajectory", rclcpp::QoS(1));
+  pub_trajectory_ = create_publisher<Trajectory>("/planning/trajectory_loader/trajectory", rclcpp::QoS(1));
   this->declare_parameter<std::string>("path_file", "path.csv");
   auto path_file = this->get_parameter("path_file").as_string();
   load_path(path_file);

src/autoware_practice_lidar_simulator/config/object_centers.csv

@@ -0,0 +1,4 @@
+x_center,y_center
+47.0,-1.0
+30.0,1.0
+15.0,-1.0

src/autoware_practice_lidar_simulator/src/simple_lidar_simulator.cpp

@@ -80,8 +80,8 @@ void SampleNode::on_timer()
     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);
+  // 車両から半径100m以内の点群を抽出
+  auto filtered_cloud = filter_points_within_radius(cloud, 100.0);
 
   sensor_msgs::msg::PointCloud2 output;
   pcl::toROSMsg(*filtered_cloud, output);