diff --git a/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp b/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp index 33cfb64..bf1a745 100644 --- a/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp +++ b/src/autoware_practice_course/src/avoidance/trajectory_planner.cpp @@ -24,7 +24,7 @@ namespace autoware_practice_course { -SampleNode::SampleNode() +TrajectoryPlannerNode::TrajectoryPlannerNode() : Node("trajectory_planner"), GRID_RESOLUTION_(1), // 1セルのサイズ（メートル） GRID_WIDTH_(100.0), // コストマップの幅（メートル） @@ -61,19 +61,19 @@ SampleNode::SampleNode() 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)); + "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&TrajectoryPlannerNode::update_current_state, this, _1)); sub_trajectory_ = create_subscription( "/planning/trajectory_loader/trajectory", rclcpp::QoS(1), - std::bind(&SampleNode::update_reference_trajectory, this, _1)); + std::bind(&TrajectoryPlannerNode::update_reference_trajectory, this, _1)); sub_pointcloud_ = create_subscription( "/perception/obstacle_segmentation/pointcloud", rclcpp::QoS(1), - std::bind(&SampleNode::update_pointcloud, this, _1)); + std::bind(&TrajectoryPlannerNode::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_ +void TrajectoryPlannerNode::update_current_state(const Odometry & msg) // called by sub_kinematic_state_ { current_velocity_ = msg.twist.twist.linear.x; current_position_ = msg.pose.pose.position; @@ -81,19 +81,20 @@ void SampleNode::update_current_state(const Odometry & msg) // called by sub_ki current_state_initialized_ = true; }; -void SampleNode::update_reference_trajectory(const Trajectory & msg) // called by sub_trajectory_ +void TrajectoryPlannerNode::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_ +void TrajectoryPlannerNode::update_pointcloud(const PointCloud2 & msg) // called by sub_pointcloud_ { pointcloud_ = msg; pointcloud_initialized_ = true; }; -void SampleNode::on_timer() +void TrajectoryPlannerNode::on_timer() { if (!current_state_initialized_ || !reference_trajectory_initialized_ || !pointcloud_initialized_) { RCLCPP_WARN(this->get_logger(), "Waiting for all initial data to be received."); @@ -114,7 +115,7 @@ void SampleNode::on_timer() } } -void SampleNode::create_trajectory() // called by on_timer() +void TrajectoryPlannerNode::create_trajectory() // called by on_timer() { // state lattice planner // create trajectory library @@ -127,7 +128,7 @@ void SampleNode::create_trajectory() // called by on_timer() best_trajectory_ = evaluate_trajectory(trajectory_set, costmap_); } -std::vector SampleNode::create_trajectory_set() +std::vector TrajectoryPlannerNode::create_trajectory_set() { // 目標状態集合を計算 std::vector target_trajectory_point_set = create_target_state_set(); @@ -178,7 +179,7 @@ std::vector SampleNode::create_trajectory_set() return trajectory_set; } -std::vector SampleNode::create_target_state_set() +std::vector TrajectoryPlannerNode::create_target_state_set() { if (reference_trajectory_.points.empty()) { RCLCPP_ERROR(this->get_logger(), "Reference trajectory is empty."); @@ -244,12 +245,12 @@ geometry_msgs::msg::Point operator+(const geometry_msgs::msg::Point & p1, const return result; } -Eigen::Vector3d SampleNode::pointToVector3d(const geometry_msgs::msg::Point & point) +Eigen::Vector3d TrajectoryPlannerNode::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 TrajectoryPlannerNode::vector3dToPoint(const Eigen::Vector3d & vector) { geometry_msgs::msg::Point point; point.x = vector.x(); @@ -258,7 +259,7 @@ geometry_msgs::msg::Point SampleNode::vector3dToPoint(const Eigen::Vector3d & ve return point; } -Eigen::Quaterniond SampleNode::vectorToQuaternion(const Eigen::Vector3d & start, const Eigen::Vector3d & end) +Eigen::Quaterniond TrajectoryPlannerNode::vectorToQuaternion(const Eigen::Vector3d & start, const Eigen::Vector3d & end) { Eigen::Vector3d direction = (end - start).normalized(); Eigen::Vector3d referenceVector(1.0, 0.0, 0.0); @@ -269,7 +270,7 @@ Eigen::Quaterniond SampleNode::vectorToQuaternion(const Eigen::Vector3d & start, return q; } -std::vector> SampleNode::create_costmap() +std::vector> TrajectoryPlannerNode::create_costmap() { pcl::PointCloud::Ptr pointcloud_pcl(new pcl::PointCloud()); // 変換関数を呼び出し @@ -309,8 +310,9 @@ std::vector> SampleNode::create_costmap() return costmap; } -SampleNode::Trajectory SampleNode::evaluate_trajectory( - const std::vector & trajectory_set, const std::vector> & costmap) +TrajectoryPlannerNode::Trajectory TrajectoryPlannerNode::evaluate_trajectory( + const std::vector & trajectory_set, + const std::vector> & costmap) { Trajectory best_trajectory; std::vector trajectory_cost(trajectory_set.size(), 0.0f); @@ -342,20 +344,20 @@ SampleNode::Trajectory SampleNode::evaluate_trajectory( return best_trajectory; } -double SampleNode::quaternionToInclination(Eigen::Quaterniond q) +double TrajectoryPlannerNode::quaternionToInclination(Eigen::Quaterniond q) { double inclination = 2.0 * q.z() / q.w(); return inclination; } -Eigen::Vector3d SampleNode::quaternionToVector(Eigen::Quaterniond q) +Eigen::Vector3d TrajectoryPlannerNode::quaternionToVector(Eigen::Quaterniond q) { Eigen::Vector3d unitVector(1, 0, 0); Eigen::Vector3d directionVector = q * unitVector; return directionVector; } // ベジエ曲線で補間する関数 -std::vector SampleNode::bezierInterpolate( +std::vector TrajectoryPlannerNode::bezierInterpolate( const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1, Eigen::Vector3d m0, Eigen::Vector3d m1) { std::vector interpolatedPoints; @@ -390,7 +392,7 @@ std::vector SampleNode::bezierInterpolate( int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + 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 index a97bda9..69a14cb 100644 --- a/src/autoware_practice_course/src/avoidance/trajectory_planner.hpp +++ b/src/autoware_practice_course/src/avoidance/trajectory_planner.hpp @@ -35,10 +35,10 @@ 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 +class TrajectoryPlannerNode : public rclcpp::Node { public: - SampleNode(); + TrajectoryPlannerNode(); private: using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; diff --git a/src/autoware_practice_course/src/vehicle/forward.cpp b/src/autoware_practice_course/src/vehicle/forward.cpp index 7a2f1e5..7f46b28 100644 --- a/src/autoware_practice_course/src/vehicle/forward.cpp +++ b/src/autoware_practice_course/src/vehicle/forward.cpp @@ -33,8 +33,8 @@ void SampleNode::on_timer() AckermannControlCommand command; command.stamp = stamp; - command.longitudinal.speed = 0.0; - command.longitudinal.acceleration = -2.5; + command.longitudinal.speed = 3.0; + command.longitudinal.acceleration = 1.0; command.lateral.steering_tire_angle = 0.0; pub_command_->publish(command); } diff --git a/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.cpp b/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.cpp index aa1bee7..72a378d 100644 --- a/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.cpp +++ b/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.cpp @@ -20,7 +20,7 @@ namespace autoware_practice_course { -SampleNode::SampleNode() : Node("longitudinal_controller"), kp_(0.0) +LongitudinalControllerNode::LongitudinalControllerNode() : Node("longitudinal_controller"), kp_(0.0) { using std::placeholders::_1; declare_parameter("kp", kp_); @@ -28,15 +28,17 @@ SampleNode::SampleNode() : Node("longitudinal_controller"), kp_(0.0) pub_command_ = create_publisher("/control/command/control_cmd", rclcpp::QoS(1)); sub_trajectory_ = create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS(1), std::bind(&SampleNode::update_target_velocity, this, _1)); + "/planning/scenario_planning/trajectory", rclcpp::QoS(1), + std::bind(&LongitudinalControllerNode::update_target_velocity, this, _1)); sub_kinematic_state_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&SampleNode::update_current_state, this, _1)); + "/localization/kinematic_state", rclcpp::QoS(1), + std::bind(&LongitudinalControllerNode::update_current_state, this, _1)); const auto period = rclcpp::Rate(10).period(); timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } -void SampleNode::update_target_velocity(const Trajectory & msg) +void LongitudinalControllerNode::update_target_velocity(const Trajectory & msg) { double min_distance = std::numeric_limits::max(); size_t closest_waypoint_index = 0; @@ -56,13 +58,13 @@ void SampleNode::update_target_velocity(const Trajectory & msg) target_velocity_ = msg.points[closest_waypoint_index].longitudinal_velocity_mps; }; -void SampleNode::update_current_state(const Odometry & msg) +void LongitudinalControllerNode::update_current_state(const Odometry & msg) { current_velocity_ = msg.twist.twist.linear.x; current_pose_ = msg.pose.pose.position; // 現在の車両の位置を更新する }; -void SampleNode::on_timer() +void LongitudinalControllerNode::on_timer() { const auto stamp = now(); @@ -84,7 +86,7 @@ void SampleNode::on_timer() int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.hpp b/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.hpp index fb373a5..5c2b899 100644 --- a/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.hpp +++ b/src/autoware_practice_course/src/velocity_planning/longitudinal_controller.hpp @@ -23,10 +23,10 @@ namespace autoware_practice_course { -class SampleNode : public rclcpp::Node +class LongitudinalControllerNode : public rclcpp::Node { public: - SampleNode(); + LongitudinalControllerNode(); private: using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; diff --git a/src/autoware_practice_course/src/velocity_planning/p_controller.cpp b/src/autoware_practice_course/src/velocity_planning/p_controller.cpp index 20c32d0..e736eb9 100644 --- a/src/autoware_practice_course/src/velocity_planning/p_controller.cpp +++ b/src/autoware_practice_course/src/velocity_planning/p_controller.cpp @@ -19,7 +19,7 @@ namespace autoware_practice_course { -SampleNode::SampleNode() : Node("p_controller") +PControllerNode::PControllerNode() : Node("p_controller") { declare_parameter("kp", 0.0); declare_parameter("target_velocity", 1.0); @@ -37,7 +37,7 @@ SampleNode::SampleNode() : Node("p_controller") timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } -void SampleNode::on_timer() +void PControllerNode::on_timer() { const auto stamp = now(); @@ -58,7 +58,7 @@ void SampleNode::on_timer() int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/src/autoware_practice_course/src/velocity_planning/p_controller.hpp b/src/autoware_practice_course/src/velocity_planning/p_controller.hpp index 5d96fb6..c06333c 100644 --- a/src/autoware_practice_course/src/velocity_planning/p_controller.hpp +++ b/src/autoware_practice_course/src/velocity_planning/p_controller.hpp @@ -22,10 +22,10 @@ namespace autoware_practice_course { -class SampleNode : public rclcpp::Node +class PControllerNode : public rclcpp::Node { public: - SampleNode(); + PControllerNode(); private: using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_follower.cpp b/src/autoware_practice_course/src/velocity_planning/trajectory_follower.cpp index 813df97..8bfee71 100644 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_follower.cpp +++ b/src/autoware_practice_course/src/velocity_planning/trajectory_follower.cpp @@ -20,7 +20,7 @@ namespace autoware_practice_course { -SampleNode::SampleNode() : Node("trajectory_follower"), kp_(0.0), lookahead_distance_(5.0) +TrajectoryFollowerNode::TrajectoryFollowerNode() : Node("trajectory_follower"), kp_(0.0), lookahead_distance_(5.0) { using std::placeholders::_1; declare_parameter("kp", kp_); @@ -34,15 +34,17 @@ SampleNode::SampleNode() : Node("trajectory_follower"), kp_(0.0), lookahead_dist pub_command_ = create_publisher("/control/command/control_cmd", rclcpp::QoS(1)); sub_trajectory_ = create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS(1), std::bind(&SampleNode::update_target_velocity, this, _1)); + "/planning/scenario_planning/trajectory", rclcpp::QoS(1), + std::bind(&TrajectoryFollowerNode::update_target_velocity, this, _1)); sub_kinematic_state_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&SampleNode::update_current_state, this, _1)); + "/localization/kinematic_state", rclcpp::QoS(1), + std::bind(&TrajectoryFollowerNode::update_current_state, this, _1)); const auto period = rclcpp::Rate(10).period(); timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } -void SampleNode::update_target_velocity(const Trajectory & msg) +void TrajectoryFollowerNode::update_target_velocity(const Trajectory & msg) { double min_distance = std::numeric_limits::max(); @@ -62,7 +64,7 @@ void SampleNode::update_target_velocity(const Trajectory & msg) target_velocity_ = msg.points[closest_point_index_].longitudinal_velocity_mps; }; -double SampleNode::load_parameters(const std::string & param_file, const std::string & param_tag) +double TrajectoryFollowerNode::load_parameters(const std::string & param_file, const std::string & param_tag) { std::ifstream file(param_file); if (!file.is_open()) { @@ -85,14 +87,14 @@ double SampleNode::load_parameters(const std::string & param_file, const std::st return -1.0; } -void SampleNode::update_current_state(const Odometry & msg) +void TrajectoryFollowerNode::update_current_state(const Odometry & msg) { current_velocity_ = msg.twist.twist.linear.x; current_position_ = msg.pose.pose.position; current_orientation_ = msg.pose.pose.orientation; }; -void SampleNode::on_timer() +void TrajectoryFollowerNode::on_timer() { const auto stamp = now(); @@ -108,12 +110,12 @@ void SampleNode::on_timer() pub_command_->publish(command); } -double SampleNode::longitudinal_controller(double velocity_error) +double TrajectoryFollowerNode::longitudinal_controller(double velocity_error) { return kp_ * velocity_error; } -double SampleNode::lateral_controller() +double TrajectoryFollowerNode::lateral_controller() { double min_distance = std::numeric_limits::max(); size_t lookahead_point_index = closest_point_index_; @@ -141,7 +143,7 @@ double SampleNode::lateral_controller() return steering_angle; } -double SampleNode::calculate_yaw_from_quaternion(const geometry_msgs::msg::Quaternion & q) +double TrajectoryFollowerNode::calculate_yaw_from_quaternion(const geometry_msgs::msg::Quaternion & q) { // Convert quaternion to Euler angles double siny_cosp = 2 * (q.w * q.z + q.x * q.y); @@ -156,7 +158,7 @@ double SampleNode::calculate_yaw_from_quaternion(const geometry_msgs::msg::Quate int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_follower.hpp b/src/autoware_practice_course/src/velocity_planning/trajectory_follower.hpp index 6282834..5ce6598 100644 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_follower.hpp +++ b/src/autoware_practice_course/src/velocity_planning/trajectory_follower.hpp @@ -28,10 +28,10 @@ namespace autoware_practice_course { -class SampleNode : public rclcpp::Node +class TrajectoryFollowerNode : public rclcpp::Node { public: - SampleNode(); + TrajectoryFollowerNode(); private: using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_loader.cpp b/src/autoware_practice_course/src/velocity_planning/trajectory_loader.cpp index d489cf1..fe1e41b 100644 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_loader.cpp +++ b/src/autoware_practice_course/src/velocity_planning/trajectory_loader.cpp @@ -23,7 +23,7 @@ namespace autoware_practice_course { -SampleNode::SampleNode() : Node("trajectory_loader") +TrajectoryLoaderNode::TrajectoryLoaderNode() : Node("trajectory_loader") { pub_trajectory_ = create_publisher("/planning/scenario_planning/trajectory", rclcpp::QoS(1)); this->declare_parameter("path_file", "path.csv"); @@ -34,12 +34,12 @@ SampleNode::SampleNode() : Node("trajectory_loader") timer_ = rclcpp::create_timer(this, get_clock(), period, [this] { on_timer(); }); } -void SampleNode::on_timer() +void TrajectoryLoaderNode::on_timer() { pub_trajectory_->publish(trajectory_); } -void SampleNode::load_path(const std::string & file_path) +void TrajectoryLoaderNode::load_path(const std::string & file_path) { std::ifstream file(file_path); if (!file.is_open()) { @@ -71,7 +71,7 @@ void SampleNode::load_path(const std::string & file_path) int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/src/autoware_practice_course/src/velocity_planning/trajectory_loader.hpp b/src/autoware_practice_course/src/velocity_planning/trajectory_loader.hpp index 56ae7e5..2a269f1 100644 --- a/src/autoware_practice_course/src/velocity_planning/trajectory_loader.hpp +++ b/src/autoware_practice_course/src/velocity_planning/trajectory_loader.hpp @@ -24,10 +24,10 @@ namespace autoware_practice_course { -class SampleNode : public rclcpp::Node +class TrajectoryLoaderNode : public rclcpp::Node { public: - SampleNode(); + TrajectoryLoaderNode(); private: using Trajectory = autoware_auto_planning_msgs::msg::Trajectory;