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);
}