Skip to content

Commit

Permalink
[vehicle_cmd_filter] minor refactor
Browse files Browse the repository at this point in the history 
Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
@TakaHoribe
TakaHoribe committed Jun 20, 2022
1 parent 31d2a3f commit bbf0f58
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 56 deletions.
31 changes: 19 additions & 12 deletions control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,18 +17,28 @@

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>

struct VehicleCmdFilterParam
{
double wheel_base;
double vel_lim;
double lon_acc_lim;
double lon_jerk_lim;
double lat_acc_lim;
double lat_jerk_lim;
};
class VehicleCmdFilter
{
public:
VehicleCmdFilter();
~VehicleCmdFilter() = default;

void setWheelBase(double v) { wheel_base_ = v; }
void setVelLim(double v) { vel_lim_ = v; }
void setLonAccLim(double v) { lon_acc_lim_ = v; }
void setLonJerkLim(double v) { lon_jerk_lim_ = v; }
void setLatAccLim(double v) { lat_acc_lim_ = v; }
void setLatJerkLim(double v) { lat_jerk_lim_ = v; }
void setWheelBase(double v) { param_.wheel_base = v; }
void setVelLim(double v) { param_.vel_lim = v; }
void setLonAccLim(double v) { param_.lon_acc_lim = v; }
void setLonJerkLim(double v) { param_.lon_jerk_lim = v; }
void setLatAccLim(double v) { param_.lat_acc_lim = v; }
void setLatJerkLim(double v) { param_.lat_jerk_lim = v; }
void setParam(const VehicleCmdFilterParam & p) { param_ = p; }
void setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v)
{
prev_cmd_ = v;
Expand All @@ -44,14 +54,11 @@ class VehicleCmdFilter
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
void limitLateralWithLatJerk(
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
void filterAll(
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;

private:
double wheel_base_;
double vel_lim_;
double lon_acc_lim_;
double lon_jerk_lim_;
double lat_acc_lim_;
double lat_jerk_lim_;
VehicleCmdFilterParam param_;
autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_;

double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const;
Expand Down
34 changes: 23 additions & 11 deletions control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,35 +21,36 @@ VehicleCmdFilter::VehicleCmdFilter() {}
void VehicleCmdFilter::limitLongitudinalWithVel(
autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
{
input.longitudinal.speed =
std::max(std::min(static_cast<double>(input.longitudinal.speed), vel_lim_), -vel_lim_);
input.longitudinal.speed = std::max(
std::min(static_cast<double>(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim);
}

void VehicleCmdFilter::limitLongitudinalWithAcc(
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
{
input.longitudinal.acceleration = std::max(
std::min(static_cast<double>(input.longitudinal.acceleration), lon_acc_lim_), -lon_acc_lim_);
std::min(static_cast<double>(input.longitudinal.acceleration), param_.lon_acc_lim),
-param_.lon_acc_lim);
input.longitudinal.speed =
limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim_ * dt);
limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt);
}

void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk(
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
{
input.longitudinal.acceleration = limitDiff(
input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim_ * dt);
input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt);
}

void VehicleCmdFilter::limitLateralWithLatAcc(
[[maybe_unused]] const double dt,
autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
{
double latacc = calcLatAcc(input);
if (std::fabs(latacc) > lat_acc_lim_) {
if (std::fabs(latacc) > param_.lat_acc_lim) {
double v_sq =
std::max(static_cast<double>(input.longitudinal.speed * input.longitudinal.speed), 0.001);
double steer_lim = std::atan(lat_acc_lim_ * wheel_base_ / v_sq);
double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq);
input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim;
}
}
Expand All @@ -60,8 +61,8 @@ void VehicleCmdFilter::limitLateralWithLatJerk(
double curr_latacc = calcLatAcc(input);
double prev_latacc = calcLatAcc(prev_cmd_);

const double latacc_max = prev_latacc + lat_jerk_lim_ * dt;
const double latacc_min = prev_latacc - lat_jerk_lim_ * dt;
const double latacc_max = prev_latacc + param_.lat_jerk_lim * dt;
const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt;

if (curr_latacc > latacc_max) {
input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max);
Expand All @@ -70,17 +71,28 @@ void VehicleCmdFilter::limitLateralWithLatJerk(
}
}

void VehicleCmdFilter::filterAll(
const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const
{
limitLongitudinalWithVel(cmd);
limitLongitudinalWithAcc(dt, cmd);
limitLongitudinalWithJerk(dt, cmd);
limitLateralWithLatAcc(dt, cmd);
limitLateralWithLatJerk(dt, cmd);
return;
}

double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const
{
const double v_sq = std::max(v * v, 0.001);
return std::atan(latacc * wheel_base_ / v_sq);
return std::atan(latacc * param_.wheel_base / v_sq);
}

double VehicleCmdFilter::calcLatAcc(
const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const
{
double v = cmd.longitudinal.speed;
return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheel_base_;
return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
}

double VehicleCmdFilter::limitDiff(
Expand Down
52 changes: 19 additions & 33 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,33 +136,26 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)

// Vehicle Parameter
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
double wheel_base = vehicle_info.wheel_base_m;
{
double vel_lim = declare_parameter("nominal.vel_lim", 25.0);
double lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0);
double lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0);
double lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0);
double lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0);
filter_.setWheelBase(wheel_base);
filter_.setVelLim(vel_lim);
filter_.setLonAccLim(lon_acc_lim);
filter_.setLonJerkLim(lon_jerk_lim);
filter_.setLatAccLim(lat_acc_lim);
filter_.setLatJerkLim(lat_jerk_lim);
VehicleCmdFilterParam p;
p.wheel_base = vehicle_info.wheel_base_m;
p.vel_lim = declare_parameter("nominal.vel_lim", 25.0);
p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0);
p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0);
p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0);
p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0);
filter_.setParam(p);
}

{
double vel_lim = declare_parameter("on_transition.vel_lim", 25.0);
double lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5);
double lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25);
double lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5);
double lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25);
filter_on_transition_.setWheelBase(wheel_base);
filter_on_transition_.setVelLim(vel_lim);
filter_on_transition_.setLonAccLim(lon_acc_lim);
filter_on_transition_.setLonJerkLim(lon_jerk_lim);
filter_on_transition_.setLatAccLim(lat_acc_lim);
filter_on_transition_.setLatJerkLim(lat_jerk_lim);
VehicleCmdFilterParam p;
p.wheel_base = vehicle_info.wheel_base_m;
p.vel_lim = declare_parameter("on_transition.vel_lim", 25.0);
p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5);
p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25);
p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5);
p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25);
filter_on_transition_.setParam(p);
}

// Set default value
Expand Down Expand Up @@ -512,19 +505,12 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
const double dt = getDt();

if (current_operation_mode_.mode == OperationMode::TRANSITION_TO_AUTO) {
filter_on_transition_.limitLongitudinalWithVel(out);
filter_on_transition_.limitLongitudinalWithAcc(dt, out);
filter_on_transition_.limitLongitudinalWithJerk(dt, out);
filter_on_transition_.limitLateralWithLatAcc(dt, out);
filter_on_transition_.limitLateralWithLatJerk(dt, out);
filter_on_transition_.filterAll(dt, out);
} else {
filter_.limitLongitudinalWithVel(out);
filter_.limitLongitudinalWithAcc(dt, out);
filter_.limitLongitudinalWithJerk(dt, out);
filter_.limitLateralWithLatAcc(dt, out);
filter_.limitLateralWithLatJerk(dt, out);
filter_.filterAll(dt, out);
}

// set prev value for both to keep consistency over switching
filter_.setPrevCmd(out);
filter_on_transition_.setPrevCmd(out);

Expand Down

0 comments on commit bbf0f58

Please sign in to comment.