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

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

Merged
merged 2 commits into from
Oct 9, 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 @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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)">
Expand All @@ -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" />
Expand All @@ -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>
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
}
Expand Down