From 9a46b63b60b6c2ec8f71e7e1c01585c10b8e6040 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Wed, 9 Oct 2024 23:49:03 +0900 Subject: [PATCH] Update mppi_controller.param.yaml and control.launch.xml --- .../planning/mppi_controller.param.yaml | 2 +- .../launch/components/control.launch.xml | 29 ++++++++++--------- .../src/simple_pd_speed_control.cpp | 7 ++--- .../simple_pure_pursuit.hpp | 2 ++ .../src/simple_pure_pursuit.cpp | 28 +++++++++++++----- 5 files changed, 42 insertions(+), 26 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index 50506a60..913df7b7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -2,7 +2,7 @@ ros__parameters: # mppi horizon : 25 - num_samples : 5000 + num_samples : 2500 u_min : [-4.0, -0.25] # accel(m/s2), steer angle(rad) u_max : [2.0, 0.25] sigmas : [2.0, 0.25] # sample range diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 7c0c285a..926aa618 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -10,20 +10,23 @@ - + + - --> + - + - @@ -61,13 +64,13 @@ - + --> - + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp index 0240e107..fb3b61c5 100644 --- a/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/control/longitudinal/simple_pd_speed_control/src/simple_pd_speed_control.cpp @@ -45,14 +45,13 @@ namespace simple_speed_pd_control{ return; } - // size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); // get closest trajectory point from current position - // TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); // calc longitudinal speed and acceleration - // double target_longitudinal_vel = closet_traj_point.longitudinal_velocity_mps; - double target_longitudinal_vel = 8.0; + double target_longitudinal_vel = closet_traj_point.longitudinal_velocity_mps; double current_longitudinal_vel = odometry_->twist.twist.linear.x; Float64 msg = Float64(); msg.data = speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp index 1d13af33..c9185a81 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp @@ -54,6 +54,8 @@ class SimplePurePursuit : public rclcpp::Node { double speed_proportional_gain_; bool use_external_target_vel_; double external_target_vel_; + double curve_param_max_steer_angle_; + double curve_param_deceleration_vel_; const double steering_tire_angle_gain_; OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index 20200939..b8f4db96 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -23,6 +23,8 @@ SimplePurePursuit::SimplePurePursuit() speed_proportional_gain_(declare_parameter("speed_proportional_gain", 1.0)), use_external_target_vel_(declare_parameter("use_external_target_vel", false)), external_target_vel_(declare_parameter("external_target_vel", 0.0)), + curve_param_max_steer_angle_(declare_parameter("curve_param_max_steer_angle", 0.1)), + curve_param_deceleration_vel_(declare_parameter("curve_param_deceleration_vel", 1.0)), steering_tire_angle_gain_(declare_parameter("steering_tire_angle_gain", 1.0)) { pub_cmd_ = create_publisher("output/control_cmd", 1); @@ -99,14 +101,6 @@ void SimplePurePursuit::onTimer() double lookahead_point_x = lookahead_point_itr->pose.position.x; double lookahead_point_y = lookahead_point_itr->pose.position.y; - // calc longitudinal speed and acceleration - double target_longitudinal_vel = - use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps; - double current_longitudinal_vel = odometry_->twist.twist.linear.x; - - cmd.longitudinal.speed = target_longitudinal_vel; - cmd.longitudinal.acceleration = - speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); { // publish lookahead point marker auto marker_msg = Marker(); @@ -142,8 +136,26 @@ void SimplePurePursuit::onTimer() steering_tire_angle_gain_ * std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); } + + // calc longitudinal speed and acceleration + double target_longitudinal_vel = + use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + + // ステアリング角度が大きい場合は減速する + if (std::abs(cmd.lateral.steering_tire_angle) > curve_param_max_steer_angle_) { + target_longitudinal_vel = std::min(target_longitudinal_vel, curve_param_deceleration_vel_); + } + + cmd.longitudinal.speed = target_longitudinal_vel; + cmd.longitudinal.acceleration = + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); + } + pub_cmd_->publish(cmd); + + // publish real-machine command cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; pub_raw_cmd_->publish(cmd); }