Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor: fix comment #52

Merged
merged 4 commits into from
Jul 19, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ void LongitudinalControllerNode::update_target_velocity(const Trajectory & msg)
void LongitudinalControllerNode::update_current_state(const Odometry & msg)
{
current_velocity_ = msg.twist.twist.linear.x;
current_pose_ = msg.pose.pose.position; // 現在の車両の位置を更新する
current_pose_ = msg.pose.pose.position;
};

void LongitudinalControllerNode::on_timer()
Expand All @@ -71,10 +71,11 @@ void LongitudinalControllerNode::on_timer()
AckermannControlCommand command;
command.stamp = stamp;

// The AckermannControlCommand can contain both longitudinal acceleration and velocity commands, but the vehicle
// interface only accepts longitudinal acceleration.
double velocity_error = target_velocity_ - current_velocity_;
command.longitudinal.acceleration = kp_ * velocity_error;
command.longitudinal.speed = target_velocity_; // メッセージ型としてはspeedがあるが、vehiclle
// interface側では加速度しか受け取っていない。
command.longitudinal.speed = target_velocity_;

command.lateral.steering_tire_angle = 0.0;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ void PControllerNode::on_timer()
AckermannControlCommand command;
command.stamp = stamp;

// The AckermannControlCommand can contain both longitudinal acceleration and velocity commands, but the vehicle
// interface only accepts longitudinal acceleration.
double velocity_error = target_velocity_ - current_velocity_;
command.longitudinal.acceleration = kp_ * velocity_error;
command.longitudinal.speed = target_velocity_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ void TrajectoryFollowerNode::update_target_velocity(const Trajectory & msg)
target_velocity_ = msg.points[closest_point_index_].longitudinal_velocity_mps;
};

// Retrieves the value of the specified parameter from the YAML file
double TrajectoryFollowerNode::load_parameters(const std::string & param_file, const std::string & param_tag)
{
std::ifstream file(param_file);
Expand Down Expand Up @@ -120,6 +121,7 @@ double TrajectoryFollowerNode::lateral_controller()
double min_distance = std::numeric_limits<double>::max();
size_t lookahead_point_index = closest_point_index_;
min_distance = std::numeric_limits<double>::max();

// Find the lookahead point
for (size_t i = closest_point_index_; i < trajectory_.points.size(); ++i) {
double dx = trajectory_.points[i].pose.position.x - current_position_.x;
Expand All @@ -145,7 +147,6 @@ double TrajectoryFollowerNode::lateral_controller()

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);
double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
double yaw = std::atan2(siny_cosp, cosy_cosp);
Expand Down
Loading