Merge pull request #58 from booars/bug-fix/pd_controller

Update mppi_controller.param.yaml and control.launch.xml

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

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml

@@ -10,20 +10,23 @@
     <let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)" />
     <let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)" />
 
-    <!-- <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
+    <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node"
       output="screen" unless="$(var use_stanley)">
       <param name="use_external_target_vel" value="false" />
       <param name="external_target_vel" value="8.3" />
-      <param name="lookahead_gain" value="0.24" />
-      <param name="lookahead_min_distance" value="2.0" />
-      <param name="speed_proportional_gain" value="2.0" />
+      <param name="lookahead_gain" value="0.25" />
+      <param name="lookahead_min_distance" value="1.6" />
+      <param name="speed_proportional_gain" value="3.0" />
       <param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>
-
+      <param name="curve_param_max_steer_angle" value="0.1" />
+      <param name="curve_param_deceleration_vel" value="4.0" />
       <remap from="input/kinematics" to="/localization/kinematic_state" />
-      <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" />
+      <!-- global path or mppi-->
       <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />
+      <!-- <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" /> -->
+
       <remap from="output/control_cmd" to="/control/command/control_cmd" />
-    </node> -->
+    </node>
 
     <!-- <node pkg="stanley_control" exec="stanley_control" name="stanley_control_node" output="screen"
       if="$(var use_stanley)">
@@ -39,14 +42,14 @@
 
     <!-- Control component -->
     <!-- Longitudinal (speed) control -->
-    <node pkg="simple_pd_speed_control" exec="simple_pd_speed_control_node" name="simple_pd_speed_control" output="screen">
+    <!--<node pkg="simple_pd_speed_control" exec="simple_pd_speed_control_node" name="simple_pd_speed_control" output="screen">
       <param name="speed_proportional_gain" value="2.0" />
       <remap from="input/kinematics" to="/localization/kinematic_state" />
       <remap from="input/trajectory" to="/planning/output/mppi_planned_path" />
-    </node>
+    </node> -->
 
     <!-- Lateral (speed) control -->
-    <node pkg="lateral_pure_pursuit" exec="lateral_pure_pursuit_node" name="lateral_pure_pursuit" 
+    <!-- <node pkg="lateral_pure_pursuit" exec="lateral_pure_pursuit_node" name="lateral_pure_pursuit" 
       output="screen" unless="$(var use_stanley)">
       <param name="wheel_base" value="1.09" />
       <param name="lookahead_gain" value="0.24" />
@@ -61,13 +64,13 @@
       <param name="k_gain_slow" value="1.0" />
       <remap from="input/kinematics" to="/localization/kinematic_state" />
       <remap from="input/trajectory" to="/planning/scenario_planning/trajectory" />
-    </node>
+    </node> -->
 
     <!-- control unifier -->
-    <node pkg="ackermann_control_publisher" exec="ackermann_control_publisher_node" name="ackermann_control_publisher" output="screen">
+    <!-- <node pkg="ackermann_control_publisher" exec="ackermann_control_publisher_node" name="ackermann_control_publisher" output="screen">
       <remap from="input/longitudinal" to="output/target_acc" />
       <remap from="input/lateral" to="output/steer_angle" />
       <remap from="output/ackermann_command" to="/control/command/control_cmd" />
-    </node>
+    </node> -->
   </group>
 </launch>

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

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_;
 

aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp

@@ -23,6 +23,8 @@ SimplePurePursuit::SimplePurePursuit()
   speed_proportional_gain_(declare_parameter<float>("speed_proportional_gain", 1.0)),
   use_external_target_vel_(declare_parameter<bool>("use_external_target_vel", false)),
   external_target_vel_(declare_parameter<float>("external_target_vel", 0.0)),
+  curve_param_max_steer_angle_(declare_parameter<float>("curve_param_max_steer_angle", 0.1)),
+  curve_param_deceleration_vel_(declare_parameter<float>("curve_param_deceleration_vel", 1.0)),
   steering_tire_angle_gain_(declare_parameter<float>("steering_tire_angle_gain", 1.0))
 {
   pub_cmd_ = create_publisher<AckermannControlCommand>("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);
 }