From 6343295a924c0afb21cc4415bb8c578c4f552e94 Mon Sep 17 00:00:00 2001 From: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com> Date: Fri, 5 Jul 2024 18:35:39 +0900 Subject: [PATCH] feat: to avoide obstacles, add state lattice planner (#45) * fix: to match the course content, change the folder stucture Signed-off-by: Masahiro Kubota * feat; add code skelton Signed-off-by: Masahiro Kubota * feat: implement all function Signed-off-by: Masahiro Kubota * fix: modify the code to build successfully Signed-off-by: Masahiro Kubota * fix: modify the code Signed-off-by: Masahiro Kubota * feat: modify the code to avoide tobjects Signed-off-by: Masahiro Kubota * feat: modify autoware.rviz to visualize the trajectories Signed-off-by: Masahiro Kubota * feat: create trajectories using bezier interpolate Signed-off-by: Masahiro Kubota * feat: add the visualization of candidate trajectories Signed-off-by: Masahiro Kubota * feat: make the marker of the best trajectory bigger Signed-off-by: Masahiro Kubota * fix: modify the calculation of the costmap Signed-off-by: Masahiro Kubota * fix: modify the marker color Signed-off-by: Masahiro Kubota * fix: visualize marker in default Signed-off-by: Masahiro Kubota * refactor: delete logging function Signed-off-by: Masahiro Kubota * refactor: arrange constant variables Signed-off-by: Masahiro Kubota * refactor: arrange constatant variables Signed-off-by: Masahiro Kubota * feat: set initial value to member constant Signed-off-by: Masahiro Kubota * refactor: set initia value Signed-off-by: Masahiro Kubota * set parameters Signed-off-by: Masahiro Kubota * modify the function name Signed-off-by: Masahiro Kubota * fix: lidar parameter Signed-off-by: Masahiro Kubota * refactor: modify comments Signed-off-by: Masahiro Kubota * fix: to adapt the code to the previous sections, modify the code Signed-off-by: Masahiro Kubota * fix: modify the cell count of Grid Signed-off-by: Masahiro Kubota * fix: add namespace Signed-off-by: Masahiro Kubota * fix: modfy the namespace Signed-off-by: Masahiro Kubota --------- Signed-off-by: Masahiro Kubota --- src/autoware_practice_course/CMakeLists.txt | 15 +- src/autoware_practice_course/package.xml | 3 + .../src/avoidance/trajectory_planner.cpp | 397 ++++++++++++++++++ .../src/avoidance/trajectory_planner.hpp | 102 +++++ .../velocity_planning/trajectory_planner.cpp | 85 ---- .../velocity_planning/trajectory_planner.hpp | 47 --- .../config/autoware.rviz | 50 ++- src/autoware_practice_msgs/CMakeLists.txt | 1 + src/autoware_practice_msgs/msg/FloatGrid.msg | 4 + .../launch/simulator.launch.xml | 1 + .../CMakeLists.txt | 2 +- .../package.xml | 2 + .../src/marker.cpp | 191 +++++++++ .../src/marker.hpp | 48 +++ 14 files changed, 813 insertions(+), 135 deletions(-) create mode 100644 src/autoware_practice_course/src/avoidance/trajectory_planner.cpp create mode 100644 src/autoware_practice_course/src/avoidance/trajectory_planner.hpp delete mode 100644 src/autoware_practice_course/src/velocity_planning/trajectory_planner.cpp delete mode 100644 src/autoware_practice_course/src/velocity_planning/trajectory_planner.hpp create mode 100644 src/autoware_practice_msgs/msg/FloatGrid.msg create mode 100644 src/autoware_practice_visualization/src/marker.cpp create mode 100644 src/autoware_practice_visualization/src/marker.hpp diff --git a/src/autoware_practice_course/CMakeLists.txt b/src/autoware_practice_course/CMakeLists.txt index 91b5032..740d739 100644 --- a/src/autoware_practice_course/CMakeLists.txt +++ b/src/autoware_practice_course/CMakeLists.txt @@ -11,15 +11,28 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) +find_package(rosidl_default_generators REQUIRED) ament_auto_find_build_dependencies() +find_package(PCL REQUIRED COMPONENTS common io) +find_package(visualization_msgs REQUIRED) + +include_directories( + include + ${PCL_INCLUDE_DIRS} +) + ament_auto_add_executable(vehicle_forward src/vehicle/forward.cpp) ament_auto_add_executable(vehicle_backward src/vehicle/backward.cpp) ament_auto_add_executable(p_controller src/velocity_planning/p_controller.cpp) -ament_auto_add_executable(trajectory_planner src/velocity_planning/trajectory_planner.cpp) ament_auto_add_executable(longitudinal_controller src/velocity_planning/longitudinal_controller.cpp) 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) + + +target_link_libraries(trajectory_planner ${PCL_LIBRARIES}) +target_link_libraries(trajectory_planner ${cpp_typesupport_target}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/autoware_practice_course/package.xml b/src/autoware_practice_course/package.xml index 9e4e204..876b8cd 100644 --- a/src/autoware_practice_course/package.xml +++ b/src/autoware_practice_course/package.xml @@ -12,9 +12,12 @@ autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_practice_msgs geometry_msgs + pcl_conversions rclcpp rclcpp_components + sensor_msgs tf2 tf2_geometry_msgs tf2_ros diff --git a/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp b/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp new file mode 100644 index 0000000..33cfb64 --- /dev/null +++ b/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp @@ -0,0 +1,397 @@ +// 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 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 "trajectory_planner.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware_practice_course +{ + +SampleNode::SampleNode() +: Node("trajectory_planner"), + GRID_RESOLUTION_(1), // 1セルのサイズ（メートル） + GRID_WIDTH_(100.0), // コストマップの幅（メートル） + GRID_HEIGHT_(100.0), // コストマップの高さ（メートル） + STATE_NUM_(9), // 目標状態の数 + TARGET_INTERVAL_(1.0), // 目標状態の間隔（メートル） + TARGET_INDEX_(10), // 目標状態までのインデックス + NUM_POINTS_(20), // ベジエ曲線による補間を分割する点の数 + CONTROL_POINT_DISTANCE_(3.0) // ベジエ曲線の端点から制御点までの距離（メートル） +{ + using std::placeholders::_1; + + declare_parameter("grid_resolution", GRID_RESOLUTION_); + declare_parameter("grid_width", GRID_WIDTH_); + declare_parameter("grid_height", GRID_HEIGHT_); + declare_parameter("state_num", STATE_NUM_); + declare_parameter("target_interval", TARGET_INTERVAL_); + declare_parameter("target_index", TARGET_INDEX_); + declare_parameter("num_points", NUM_POINTS_); + declare_parameter("control_point_distance", CONTROL_POINT_DISTANCE_); + + get_parameter("grid_resolution", GRID_RESOLUTION_); + get_parameter("grid_width", GRID_WIDTH_); + get_parameter("grid_height", GRID_HEIGHT_); + get_parameter("state_num", STATE_NUM_); + get_parameter("target_interval", TARGET_INTERVAL_); + get_parameter("target_index", TARGET_INDEX_); + get_parameter("num_points", NUM_POINTS_); + get_parameter("control_point_distance", CONTROL_POINT_DISTANCE_); + + pub_trajectory_ = create_publisher("/planning/scenario_planning/trajectory", rclcpp::QoS(1)); + pub_trajectory_candidate_ = + create_publisher("/planning/scenario_planning/trajectory_candidate", rclcpp::QoS(1)); + pub_costmap_ = + create_publisher("/planning/scenario_planning/costmap", rclcpp::QoS(1)); + sub_kinematic_state_ = create_subscription( + "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&SampleNode::update_current_state, this, _1)); + sub_trajectory_ = create_subscription( + "/planning/trajectory_loader/trajectory", rclcpp::QoS(1), + std::bind(&SampleNode::update_reference_trajectory, this, _1)); + sub_pointcloud_ = create_subscription( + "/perception/obstacle_segmentation/pointcloud", rclcpp::QoS(1), + std::bind(&SampleNode::update_pointcloud, this, _1)); + + const auto period = rclcpp::Rate(10).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); +} + +void SampleNode::update_current_state(const Odometry & msg) // called by sub_kinematic_state_ +{ + current_velocity_ = msg.twist.twist.linear.x; + current_position_ = msg.pose.pose.position; + current_orientation_ = msg.pose.pose.orientation; + current_state_initialized_ = true; +}; + +void SampleNode::update_reference_trajectory(const Trajectory & msg) // called by sub_trajectory_ +{ + reference_trajectory_ = msg; + reference_trajectory_initialized_ = true; +}; + +void SampleNode::update_pointcloud(const PointCloud2 & msg) // called by sub_pointcloud_ +{ + pointcloud_ = msg; + pointcloud_initialized_ = true; +}; + +void SampleNode::on_timer() +{ + if (!current_state_initialized_ || !reference_trajectory_initialized_ || !pointcloud_initialized_) { + RCLCPP_WARN(this->get_logger(), "Waiting for all initial data to be received."); + } else { + create_trajectory(); + pub_trajectory_->publish(best_trajectory_); + pub_trajectory_candidate_->publish(trajectory_candidate_); + auto costmap_msg = autoware_practice_msgs::msg::FloatGrid(); + costmap_msg.width = GRID_WIDTH_; + costmap_msg.height = GRID_HEIGHT_; + + // 2次元配列を1次元配列に変換 + for (const auto & row : costmap_) { + costmap_msg.data.insert(costmap_msg.data.end(), row.begin(), row.end()); + } + + pub_costmap_->publish(costmap_msg); + } +} + +void SampleNode::create_trajectory() // called by on_timer() +{ + // state lattice planner + // create trajectory library + std::vector trajectory_set = create_trajectory_set(); + + // create costmap + costmap_ = create_costmap(); + + // evaluate trahectories by the cost map + best_trajectory_ = evaluate_trajectory(trajectory_set, costmap_); +} + +std::vector SampleNode::create_trajectory_set() +{ + // 目標状態集合を計算 + std::vector target_trajectory_point_set = create_target_state_set(); + + if (target_trajectory_point_set.size() < 2) { + RCLCPP_ERROR(this->get_logger(), "Target trajectory point set size is less than 2."); + return {}; + } + + std::vector trajectory_set; + Eigen::Quaterniond current_q; + Eigen::Quaterniond target_q; + + current_q.x() = current_orientation_.x; + current_q.y() = current_orientation_.y; + current_q.z() = current_orientation_.z; + current_q.w() = current_orientation_.w; + target_q.x() = target_trajectory_point_set[1].pose.orientation.x; + target_q.y() = target_trajectory_point_set[1].pose.orientation.y; + target_q.z() = target_trajectory_point_set[1].pose.orientation.z; + target_q.w() = target_trajectory_point_set[1].pose.orientation.w; + Eigen::Vector3d current_vector = quaternionToVector(current_q); + Eigen::Vector3d target_vector = quaternionToVector(target_q); + + // 車両の位置姿勢と目標状態集合をエルミート補間し、軌道を生成 + Trajectory trajectory_candidate; + for (const auto & target_trajectory_point : target_trajectory_point_set) { + // 車両の位置姿勢と目標状態をベジエ曲線で補間 + std::vector interpolated_points = + bezierInterpolate(current_position_, target_trajectory_point.pose.position, current_vector, target_vector); + + Trajectory trajectorys; + + // 補間された曲線状のstd::vectorをTrajectoryに変換 + for (const auto & interpolated_point : interpolated_points) { + TrajectoryPoint trajectory_point; + trajectory_point.pose.position = interpolated_point; + trajectory_point.longitudinal_velocity_mps = 2.0; + trajectorys.points.push_back(trajectory_point); + trajectory_candidate.points.push_back(trajectory_point); + } + + // 補間された曲線状のTrajectory Pointを生成 + trajectory_set.push_back(trajectorys); + } + trajectory_candidate_ = trajectory_candidate; + + return trajectory_set; +} + +std::vector SampleNode::create_target_state_set() +{ + if (reference_trajectory_.points.empty()) { + RCLCPP_ERROR(this->get_logger(), "Reference trajectory is empty."); + return {}; + } else { + // 車両に最も近いtrajectory pointを取得 + double min_distance = std::numeric_limits::max(); + size_t closest_waypoint_index = 0; + + for (size_t i = 0; i < reference_trajectory_.points.size(); ++i) { + double dx = reference_trajectory_.points[i].pose.position.x - current_position_.x; + double dy = reference_trajectory_.points[i].pose.position.y - current_position_.y; + double dz = reference_trajectory_.points[i].pose.position.z - current_position_.z; + double distance = std::sqrt(dx * dx + dy * dy + dz * dz); + + if (distance < min_distance) { + min_distance = distance; + closest_waypoint_index = i; + } + } + + int target_trajectory_point_index = closest_waypoint_index + TARGET_INDEX_; + + // TARGET_INDEX_個先のtrajectory pointを取得 + autoware_auto_planning_msgs::msg::TrajectoryPoint target_trajectory_point = + reference_trajectory_.points[target_trajectory_point_index]; + + Eigen::Quaterniond q = vectorToQuaternion( + pointToVector3d(reference_trajectory_.points[target_trajectory_point_index].pose.position), + pointToVector3d(reference_trajectory_.points[target_trajectory_point_index + 1].pose.position)); + target_trajectory_point.pose.orientation.x = q.x(); + target_trajectory_point.pose.orientation.y = q.y(); + target_trajectory_point.pose.orientation.z = q.z(); + target_trajectory_point.pose.orientation.w = q.w(); + + // target_trajectory_pointの姿勢に直交する方向に並ぶSTATE_NUM_個の状態を生成 + // クォータニオンを回転行列に変換 + Eigen::Matrix3d R = q.toRotationMatrix(); + + // 直交するベクトルの選択（第二列ベクトル） + Eigen::Vector3d v = R.col(1); + + std::vector target_state_set; + for (int n = -STATE_NUM_ / 2; n <= (STATE_NUM_ - STATE_NUM_ / 2 - 1); ++n) { + autoware_auto_planning_msgs::msg::TrajectoryPoint target_state = target_trajectory_point; + target_state.pose.position.x += n * TARGET_INTERVAL_ * v.x(); + target_state.pose.position.y += n * TARGET_INTERVAL_ * v.y(); + target_state.pose.position.z += n * TARGET_INTERVAL_ * v.z(); + target_state_set.push_back(target_state); + } + + return target_state_set; + } +} + +// + 演算子のオーバーロード +geometry_msgs::msg::Point operator+(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point result; + result.x = p1.x + p2.x; + result.y = p1.y + p2.y; + result.z = p1.z + p2.z; + return result; +} + +Eigen::Vector3d SampleNode::pointToVector3d(const geometry_msgs::msg::Point & point) +{ + return Eigen::Vector3d(point.x, point.y, point.z); +} + +geometry_msgs::msg::Point SampleNode::vector3dToPoint(const Eigen::Vector3d & vector) +{ + geometry_msgs::msg::Point point; + point.x = vector.x(); + point.y = vector.y(); + point.z = vector.z(); + return point; +} + +Eigen::Quaterniond SampleNode::vectorToQuaternion(const Eigen::Vector3d & start, const Eigen::Vector3d & end) +{ + Eigen::Vector3d direction = (end - start).normalized(); + Eigen::Vector3d referenceVector(1.0, 0.0, 0.0); + Eigen::Vector3d axis = referenceVector.cross(direction); + axis.normalize(); + double angle = acos(referenceVector.dot(direction)); + Eigen::Quaterniond q(Eigen::AngleAxisd(angle, axis)); + return q; +} + +std::vector> SampleNode::create_costmap() +{ + pcl::PointCloud::Ptr pointcloud_pcl(new pcl::PointCloud()); + // 変換関数を呼び出し + pcl::fromROSMsg(pointcloud_, *pointcloud_pcl); + // pointcloud_を元にcostmapを生成 + + std::vector> costmap(GRID_WIDTH_, std::vector(GRID_HEIGHT_, 0.0)); + + // 点群をグリッドマップに変換 + for (const auto & point : pointcloud_pcl->points) { + int x_index = static_cast(point.x / GRID_RESOLUTION_); + int y_index = static_cast((point.y + GRID_WIDTH_ / 2) / GRID_RESOLUTION_); + const int KERNEL_SIZE = 1; // 評価関数を設定する範囲（カーネルサイズ） + const float SURROUNDING_COST = 50.0; // 周囲の格子の評価値 + if (x_index >= 0 && x_index < GRID_WIDTH_ && y_index >= 0 && y_index < GRID_HEIGHT_) { + costmap[x_index][y_index] += 100.0; // 点が存在する格子は評価関数を高く設定 + for (int x = x_index - KERNEL_SIZE; x <= x_index + KERNEL_SIZE; ++x) { + for (int y = y_index - KERNEL_SIZE; y <= y_index + KERNEL_SIZE; ++y) { + if (x >= 0 && x < GRID_WIDTH_ && y >= 0 && y < GRID_HEIGHT_) { + costmap[x][y] += SURROUNDING_COST; + } + } + } + } + } + + const float REFERENCE_TRAJECTORY_COST = -1; // reference_trajectoryの格子の評価値 + for (const auto & point : reference_trajectory_.points) { + int x_index = static_cast(point.pose.position.x / GRID_RESOLUTION_); + int y_index = static_cast((point.pose.position.y + GRID_WIDTH_ / 2) / GRID_RESOLUTION_); + + if (x_index >= 0 && x_index < GRID_WIDTH_ && y_index >= 0 && y_index < GRID_HEIGHT_) { + costmap[x_index][y_index] += REFERENCE_TRAJECTORY_COST; + } + } + + return costmap; +} + +SampleNode::Trajectory SampleNode::evaluate_trajectory( + const std::vector & trajectory_set, const std::vector> & costmap) +{ + Trajectory best_trajectory; + std::vector trajectory_cost(trajectory_set.size(), 0.0f); + size_t index = 0; + + // trajectory_setをcostmapで評価し、最適な軌道を選択 + for (const auto & trajectory_candidate : trajectory_set) { + // trajectoryをcostmapで評価 + for (const auto & trajectory_point : trajectory_candidate.points) { + int x_index = static_cast(trajectory_point.pose.position.x / GRID_RESOLUTION_); + int y_index = static_cast((trajectory_point.pose.position.y + GRID_WIDTH_ / 2) / GRID_RESOLUTION_); + + 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( + this->get_logger(), "Trajectory point out of costmap bounds: x_index=%d, y_index=%d", x_index, y_index); + } + } + + index++; + } + + // 最小コストのインデックスを出力 + int min_index = + std::distance(trajectory_cost.begin(), std::min_element(trajectory_cost.begin(), trajectory_cost.end())); + best_trajectory = trajectory_set[min_index]; + + return best_trajectory; +} + +double SampleNode::quaternionToInclination(Eigen::Quaterniond q) +{ + double inclination = 2.0 * q.z() / q.w(); + return inclination; +} + +Eigen::Vector3d SampleNode::quaternionToVector(Eigen::Quaterniond q) +{ + Eigen::Vector3d unitVector(1, 0, 0); + Eigen::Vector3d directionVector = q * unitVector; + return directionVector; +} +// ベジエ曲線で補間する関数 +std::vector SampleNode::bezierInterpolate( + const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1, Eigen::Vector3d m0, Eigen::Vector3d m1) +{ + std::vector interpolatedPoints; + + Eigen::Vector3d v0 = pointToVector3d(p0); + Eigen::Vector3d v1 = pointToVector3d(p1); + + // Control points + Eigen::Vector3d c0 = v0; + Eigen::Vector3d c1 = v0 + m0 * CONTROL_POINT_DISTANCE_; + Eigen::Vector3d c2 = v1 - m1 * CONTROL_POINT_DISTANCE_; + Eigen::Vector3d c3 = v1; + + for (int i = 0; i <= NUM_POINTS_; ++i) { + double t = static_cast(i) / NUM_POINTS_; + double t2 = t * t; + double t3 = t2 * t; + double one_minus_t = 1.0 - t; + double one_minus_t2 = one_minus_t * one_minus_t; + double one_minus_t3 = one_minus_t2 * one_minus_t; + + Eigen::Vector3d point = one_minus_t3 * c0 + 3.0 * one_minus_t2 * t * c1 + 3.0 * one_minus_t * t2 * c2 + t3 * c3; + + interpolatedPoints.push_back(vector3dToPoint(point)); + } + + return interpolatedPoints; +} + +} // namespace autoware_practice_course + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/autoware_practice_course/src/avoidance/trajectory_planner.hpp b/src/autoware_practice_course/src/avoidance/trajectory_planner.hpp new file mode 100644 index 0000000..a97bda9 --- /dev/null +++ b/src/autoware_practice_course/src/avoidance/trajectory_planner.hpp @@ -0,0 +1,102 @@ +// 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 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 AVOIDANCE__TRAJECTORY_PLANNER_HPP_ +#define AVOIDANCE__TRAJECTORY_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +// フリー関数としての + 演算子のオーバーロードの宣言 + +namespace autoware_practice_course +{ +geometry_msgs::msg::Point operator+(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + +class SampleNode : public rclcpp::Node +{ +public: + SampleNode(); + +private: + using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; + using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; + using Odometry = nav_msgs::msg::Odometry; + using Point = geometry_msgs::msg::Point; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Quaternion = geometry_msgs::msg::Quaternion; + + void on_timer(); + void update_current_state(const Odometry & msg); + void update_reference_trajectory(const Trajectory & msg); + void update_pointcloud(const PointCloud2 & msg); + void create_trajectory(); + std::vector create_trajectory_set(); + TrajectoryPoint calculate_target_trajectory_point(); + std::vector> create_costmap(); + Trajectory evaluate_trajectory( + const std::vector & trajectory_set, const std::vector> & costmap); + std::vector create_target_state_set(); + std::vector bezierInterpolate(const Point & p0, const Point & p1, Eigen::Vector3d m0, Eigen::Vector3d m1); + double quaternionToInclination(Eigen::Quaterniond q); + Eigen::Vector3d quaternionToVector(Eigen::Quaterniond q); + Eigen::Vector3d pointToVector3d(const geometry_msgs::msg::Point & point); + geometry_msgs::msg::Point vector3dToPoint(const Eigen::Vector3d & vector); + Eigen::Quaterniond vectorToQuaternion(const Eigen::Vector3d & start, const Eigen::Vector3d & end); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_candidate_; + rclcpp::Publisher::SharedPtr pub_costmap_; + rclcpp::Subscription::SharedPtr sub_kinematic_state_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_pointcloud_; + + double GRID_RESOLUTION_; + double GRID_WIDTH_; // 目標状態の間隔（メートル） + double GRID_HEIGHT_; // 目標状態までのインデックス + int STATE_NUM_; // ベジエ曲線による補間を分割する点の数 + double TARGET_INTERVAL_; // ベジエ曲線の端点から制御点までの距離（メートル） + int TARGET_INDEX_; // コストマップの幅（メートル） + int NUM_POINTS_; // コストマップの高さ（メートル） + double CONTROL_POINT_DISTANCE_; // 目標状態の数 + + PointCloud2 pointcloud_; + Point current_position_; + Quaternion current_orientation_; + double current_velocity_; + Trajectory reference_trajectory_; + Trajectory best_trajectory_; + Trajectory trajectory_candidate_; + std::vector> costmap_; + + bool current_state_initialized_ = false; + bool reference_trajectory_initialized_ = false; + bool pointcloud_initialized_ = false; +}; + +} // namespace autoware_practice_course + +#endif // AVOIDANCE__TRAJECTORY_PLANNER_HPP_ diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_planner.cpp b/src/autoware_practice_course/src/velocity_planning/trajectory_planner.cpp deleted file mode 100644 index 9ac3d15..0000000 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_planner.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// 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 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 "trajectory_planner.hpp" - -#include -#include - -namespace autoware_practice_course -{ - -SampleNode::SampleNode() : Node("trajectory_planner") -{ - using std::placeholders::_1; - - goal_ = 100.0; - declare_parameter("goal", 100.0); - get_parameter("goal", goal_); - pub_trajectory_ = create_publisher("/planning/scenario_planning/trajectory", rclcpp::QoS(1)); - sub_kinematic_state_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&SampleNode::update_vehicle_position, this, _1)); - - const auto period = rclcpp::Rate(10).period(); - timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); -} - -void SampleNode::update_vehicle_position(const Odometry & msg) -{ - position_x_ = msg.pose.pose.position.x; -} - -void SampleNode::on_timer() -{ - const auto stamp = now(); - - Trajectory trajectory; - trajectory.header.stamp = stamp; - trajectory.header.frame_id = "map"; - int distance = static_cast(std::floor(goal_)); - - for (int i = 1; i <= distance; ++i) { - TrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(i); - point.time_from_start.nanosec = 0; - point.pose.position.x = static_cast(i); - point.pose.position.y = 0.0; - point.pose.position.z = 0.0; - point.pose.orientation.x = 0.0; - point.pose.orientation.y = 0.0; - point.pose.orientation.z = 0.0; - point.pose.orientation.w = 1.0; - double waypoint_i_x = static_cast(i); - point.longitudinal_velocity_mps = (waypoint_i_x < 50) ? (0.2 * waypoint_i_x) : (-0.2 * waypoint_i_x + 20.0); - point.lateral_velocity_mps = 0.0; - point.acceleration_mps2 = 0.0; - point.heading_rate_rps = 0.0; - point.front_wheel_angle_rad = 0.0; - point.rear_wheel_angle_rad = 0.0; - trajectory.points.push_back(point); - } - - pub_trajectory_->publish(trajectory); -} - -} // namespace autoware_practice_course - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_planner.hpp b/src/autoware_practice_course/src/velocity_planning/trajectory_planner.hpp deleted file mode 100644 index ee97b38..0000000 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_planner.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// 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 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 VELOCITY_PLANNING__TRAJECTORY_PLANNER_HPP_ -#define VELOCITY_PLANNING__TRAJECTORY_PLANNER_HPP_ - -#include -#include -#include -#include - -namespace autoware_practice_course -{ - -class SampleNode : public rclcpp::Node -{ -public: - SampleNode(); - -private: - using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; - using Odometry = nav_msgs::msg::Odometry; - - void on_timer(); - void update_vehicle_position(const Odometry & msg); - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr pub_trajectory_; - rclcpp::Subscription::SharedPtr sub_kinematic_state_; - double goal_; - double position_x_; -}; - -} // namespace autoware_practice_course - -#endif // VELOCITY_PLANNING__TRAJECTORY_PLANNER_HPP_ diff --git a/src/autoware_practice_launch/config/autoware.rviz b/src/autoware_practice_launch/config/autoware.rviz index 0fb4c46..32758e0 100644 --- a/src/autoware_practice_launch/config/autoware.rviz +++ b/src/autoware_practice_launch/config/autoware.rviz @@ -44,7 +44,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 20 + Plane Cell Count: 40 Reference Frame: Value: true - Class: rviz_default_plugins/MarkerArray @@ -59,6 +59,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /simulator/markers Value: true + - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray @@ -82,6 +83,53 @@ Visualization Manager: Value: /sensing/lidar/concatenated/pointcloud Size (m): 0.1 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrajectoryMarkers + Namespaces: {} + Topic: + Value: /trajectory_marker + History Policy: Keep Last + Reliability Policy: Reliable + Qos Overrides: + /parameter_events: + Depth: 1000 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: true + + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrajectoryMarkers + Namespaces: {} + Topic: + Value: /reference_trajectory_marker + History Policy: Keep Last + Reliability Policy: Reliable + Qos Overrides: + /parameter_events: + Depth: 1000 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: true + + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrajectoryMarkers + Namespaces: {} + Topic: + Value: /candidate_trajectory_marker + History Policy: Keep Last + Reliability Policy: Reliable + Qos Overrides: + /parameter_events: + Depth: 1000 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: true Enabled: true Global Options: diff --git a/src/autoware_practice_msgs/CMakeLists.txt b/src/autoware_practice_msgs/CMakeLists.txt index 01a48a1..58f660f 100644 --- a/src/autoware_practice_msgs/CMakeLists.txt +++ b/src/autoware_practice_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ ament_auto_find_build_dependencies() rosidl_generate_interfaces(${PROJECT_NAME} "msg/JudgeStatus.msg" + "msg/FloatGrid.msg" ) ament_auto_package() diff --git a/src/autoware_practice_msgs/msg/FloatGrid.msg b/src/autoware_practice_msgs/msg/FloatGrid.msg new file mode 100644 index 0000000..03911dd --- /dev/null +++ b/src/autoware_practice_msgs/msg/FloatGrid.msg @@ -0,0 +1,4 @@ +# FloatGrid.msg +float32[] data +uint32 width +uint32 height diff --git a/src/autoware_practice_simulator/launch/simulator.launch.xml b/src/autoware_practice_simulator/launch/simulator.launch.xml index aa35ede..645b15f 100644 --- a/src/autoware_practice_simulator/launch/simulator.launch.xml +++ b/src/autoware_practice_simulator/launch/simulator.launch.xml @@ -5,4 +5,5 @@ + diff --git a/src/autoware_practice_visualization/CMakeLists.txt b/src/autoware_practice_visualization/CMakeLists.txt index dcb3b05..bebd061 100644 --- a/src/autoware_practice_visualization/CMakeLists.txt +++ b/src/autoware_practice_visualization/CMakeLists.txt @@ -15,7 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) - +ament_auto_add_executable(marker src/marker.cpp) pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) ament_auto_package() diff --git a/src/autoware_practice_visualization/package.xml b/src/autoware_practice_visualization/package.xml index dbb13ea..c4dbeac 100644 --- a/src/autoware_practice_visualization/package.xml +++ b/src/autoware_practice_visualization/package.xml @@ -9,12 +9,14 @@ ament_cmake_auto + autoware_auto_planning_msgs autoware_practice_msgs libqt5-core libqt5-gui libqt5-widgets rclcpp rviz_common + visualization_msgs ament_cmake_copyright ament_cmake_cppcheck diff --git a/src/autoware_practice_visualization/src/marker.cpp b/src/autoware_practice_visualization/src/marker.cpp new file mode 100644 index 0000000..bbb4c30 --- /dev/null +++ b/src/autoware_practice_visualization/src/marker.cpp @@ -0,0 +1,191 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "marker.hpp" + +#include +namespace autoware_practice_visualization +{ + +TrajectoryVisualizer::TrajectoryVisualizer(const rclcpp::NodeOptions & options) : Node("marker", options) +{ + trajectory_sub_ = this->create_subscription( + "/planning/scenario_planning/trajectory", 10, + std::bind(&TrajectoryVisualizer::trajectoryCallback, this, std::placeholders::_1)); + reference_trajectory_sub_ = this->create_subscription( + "/planning/trajectory_loader/trajectory", 10, + std::bind(&TrajectoryVisualizer::reference_trajectoryCallback, this, std::placeholders::_1)); + trajectory_candidate_sub_ = this->create_subscription( + "/planning/scenario_planning/trajectory_candidate", 10, + std::bind(&TrajectoryVisualizer::trajectory_candidateCallback, this, std::placeholders::_1)); + costmap_sub_ = this->create_subscription( + "/planning/scenario_planning/costmap", 10, + std::bind(&TrajectoryVisualizer::costmapCallback, this, std::placeholders::_1)); + + marker_pub_ = this->create_publisher("/trajectory_marker", 10); + reference_marker_pub_ = + this->create_publisher("/reference_trajectory_marker", 10); + candidate_marker_pub_ = + this->create_publisher("/candidate_trajectory_marker", 10); + costmap_marker_pub_ = this->create_publisher("/costmap_marker", 10); +} + +void TrajectoryVisualizer::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.5; + marker.scale.y = 0.5; + marker.scale.z = 0.5; + + 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 TrajectoryVisualizer::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 = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + marker_array.markers.push_back(marker); + } + + reference_marker_pub_->publish(marker_array); +} + +void TrajectoryVisualizer::trajectory_candidateCallback( + 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 = 0.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + + marker_array.markers.push_back(marker); + } + + candidate_marker_pub_->publish(marker_array); +} + +void TrajectoryVisualizer::costmapCallback(const autoware_practice_msgs::msg::FloatGrid::SharedPtr msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + float resolution = 1.0; // コストマップの解像度（必要に応じて設定） + + for (size_t y = 0; y < msg->height; ++y) { + for (size_t x = 0; x < msg->width; ++x) { + size_t index = y + x * msg->height; + if (index >= msg->data.size()) continue; // 安全対策 + float cost = msg->data[index]; + if (cost > 100.0) { + RCLCPP_INFO(this->get_logger(), "index: %ld cost: %f", index, cost); // コスト値の表示（デバッグ用 + } + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; // 適切なフレームIDを設定 + marker.ns = "costmap"; + marker.id = index; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = x * resolution; + marker.pose.position.y = y * resolution - 50; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + + marker.scale.x = resolution; + marker.scale.y = resolution; + marker.scale.z = 0.1; + + marker.color.r = cost / 1000; // コスト値に基づく色の設定（適宜調整） + marker.color.g = 1.0 - cost / 1000; + marker.color.b = 0.0; + marker.color.a = 0.5; + + marker_array.markers.push_back(marker); + } + } + + candidate_marker_pub_->publish(marker_array); +} + +} // namespace autoware_practice_visualization + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto options = rclcpp::NodeOptions(); + auto node = std::make_shared(options); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/autoware_practice_visualization/src/marker.hpp b/src/autoware_practice_visualization/src/marker.hpp new file mode 100644 index 0000000..f0f7244 --- /dev/null +++ b/src/autoware_practice_visualization/src/marker.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 MARKER_HPP_ +#define MARKER_HPP_ + +#include +#include +#include +#include + +namespace autoware_practice_visualization +{ +class TrajectoryVisualizer : public rclcpp::Node +{ +public: + explicit TrajectoryVisualizer(const rclcpp::NodeOptions & options); + +private: + void trajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void reference_trajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void trajectory_candidateCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void costmapCallback(const autoware_practice_msgs::msg::FloatGrid::SharedPtr msg); + + rclcpp::Subscription::SharedPtr trajectory_sub_; + rclcpp::Subscription::SharedPtr reference_trajectory_sub_; + rclcpp::Subscription::SharedPtr trajectory_candidate_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr reference_marker_pub_; + rclcpp::Publisher::SharedPtr candidate_marker_pub_; + rclcpp::Publisher::SharedPtr costmap_marker_pub_; +}; + +} // namespace autoware_practice_visualization + +#endif // MARKER_HPP_