autoware之twist_filter节点

  twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this);
  ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this);
  config_sub_ = nh_.subscribe("config/twist_filter", 10, &TwistFilterNode::configCallback, this);

  // Publish
  twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_cmd", 5);
  ctrl_pub_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 5);

1.configCallback函数

void TwistFilterNode::configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr& config_msg)
{
  twist_filter::Configuration twist_filter_config;
  twist_filter_config.lateral_accel_limit = config_msg->lateral_accel_limit;
  twist_filter_config.lateral_jerk_limit = config_msg->lateral_jerk_limit;
  twist_filter_config.lowpass_gain_linear_x = config_msg->lowpass_gain_linear_x;
  twist_filter_config.lowpass_gain_angular_z = config_msg->lowpass_gain_angular_z;
  twist_filter_config.lowpass_gain_steering_angle = config_msg->lowpass_gain_steering_angle;
  twist_filter_ptr_->setConfiguration(twist_filter_config);
}

函数接收config/twist_filter话题中消息内容，把对应参数值传递给
lateral_accel_limit 横向加速度限制
lateral_jerk_limit 横向抖动限制
lowpass_gain_linear_x 速度的低通滤波系数
lowpass_gain_angular_z 角速度的低通滤波系数
lowpass_gain_steering_angle 转角的低通滤波系数

2.twistCmdCallback函数

void TwistFilterNode::twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg)
{
  const twist_filter::Twist twist = { msg->twist.linear.x, msg->twist.angular.z };
  ros::Time current_time = ros::Time::now();

  static ros::Time last_callback_time = current_time;
  static twist_filter::Twist twist_prev = twist;

  double time_elapsed = (current_time - last_callback_time).toSec();

  health_checker_.NODE_ACTIVATE();
  checkTwist(twist, twist_prev, time_elapsed);

  twist_filter::Twist twist_out = twist;

  // Apply lateral limit
  auto twist_limit_result = twist_filter_ptr_->lateralLimitTwist(twist, twist_prev, time_elapsed);
  if (twist_limit_result)
  {
    twist_out = twist_limit_result.get();
  }

  // Publish lateral accel and jerk before smoothing//
  auto lacc_no_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist);
  if (lacc_no_smoothed_result)
  {
    std_msgs::Float32 lacc_msg_debug;
    lacc_msg_debug.data = lacc_no_smoothed_result.get();
    twist_lacc_limit_debug_pub_.publish(lacc_msg_debug);
  }
  auto ljerk_no_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist, twist_prev, time_elapsed);
  if (ljerk_no_smoothed_result)
  {
    std_msgs::Float32 ljerk_msg_debug;
    ljerk_msg_debug.data = ljerk_no_smoothed_result.get();
    twist_ljerk_limit_debug_pub_.publish(ljerk_msg_debug);
  }

  // Smoothing
  twist_out = twist_filter_ptr_->smoothTwist(twist_out);

  // Smoothed value publish
  geometry_msgs::TwistStamped out_msg = *msg;
  out_msg.twist.linear.x = twist_out.lx;
  out_msg.twist.angular.z = twist_out.az;
  twist_pub_.publish(out_msg);

  // Publish lateral accel and jerk after smoothing
  auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist_out);
  if (lacc_smoothed_result)
  {
    std_msgs::Float32 lacc_msg;
    lacc_msg.data = lacc_smoothed_result.get();
    twist_lacc_result_pub_.publish(lacc_msg);
  }
  auto ljerk_smoothed_result = twist_filter_ptr_->calcLjerkWithAngularZ(twist_out, twist_prev, time_elapsed);
  if (ljerk_smoothed_result)
  {
    std_msgs::Float32 ljerk_msg;
    ljerk_msg.data = ljerk_smoothed_result.get();
    twist_ljerk_result_pub_.publish(ljerk_msg);
  }

  // Preserve value and time
  twist_prev = twist_out;
  last_callback_time = current_time;
}

接收到的消息包含线速度和角速度

  // Apply lateral limit
  auto twist_limit_result = twist_filter_ptr_->lateralLimitTwist(twist, twist_prev, time_elapsed);
  if (twist_limit_result)
  {
    twist_out = twist_limit_result.get();
  }

关于lateralLimitTwist函数

boost::optional<Twist> TwistFilter::lateralLimitTwist(const Twist& twist, const Twist& twist_prev,
                                                      const double& dt) const
{
  static bool init = false;

  Twist twist_out = twist;

  // skip first msg, check linear_velocity
  //X轴线速度接近于0，车辆停止，直接返回
  const bool is_stopping = (std::fabs(twist.lx) < MIN_LINEAR_X);
  if (!init || is_stopping)
  {
    init = true;
    return twist_out;
  }

  // lateral acceleration
  //计算横向加速度 twist.az * twist.lx
  auto lacc_result = calcLaccWithAngularZ(twist_out);
  // limit lateral acceleration
  if (lacc_result)
  {
  	//横向加速度大于设定限值lateral_accel_limit且车辆非静止状态，则角速度赋值为az_max，线速度仍为接收的线速度。
    if (std::fabs(lacc_result.get()) + std::numeric_limits<double>::epsilon() > config_.lateral_accel_limit &&
        !is_stopping)
    {
      double sgn = lacc_result.get() / std::fabs(lacc_result.get());
      double az_max = sgn * (config_.lateral_accel_limit) / twist_out.lx;
      twist_out.az = az_max;
    }
  }
  else
  {
    return boost::none;
  }

  // lateral jerk
  /*
   jerk抖动也即 (twist.az - twist_prev.az) * twist.lx / dt;横向加速度变化率
  */
  auto ljerk_result = calcLjerkWithAngularZ(twist_out, twist_prev, dt);
  // limit lateral jerk
  if (ljerk_result)
  {
    if (std::fabs(ljerk_result.get()) + std::numeric_limits<double>::epsilon() > config_.lateral_jerk_limit &&
        !is_stopping)
    {
      double sgn = ljerk_result.get() / std::fabs(ljerk_result.get());
      double az_max = twist_prev.az + (sgn * config_.lateral_jerk_limit / twist_out.lx) * dt;
      twist_out.az = az_max;
    }
  }
  else
  {
    return boost::none;
  }
  return twist_out;
}

接着对输出的速度和角速度进行一阶低通滤波处理
一阶低通滤波的算法公式：
Y(n)=aX(n)+(1-a)Y(n-1)
其中，
a为滤波系数；
X(n)为本次采样值；
Y(n-1)为上次滤波输出值；
Y(n)为本次滤波输出值；
也就是说一阶低通滤波算法采样本次采样值与上次滤波输出值进行加权，得到有效滤波值。

auto lowpass_filter()
{
  double y = 0.0;
  return [y](const double& x, const double& gain) mutable -> double
  {
    y = gain * y + (1 - gain) * x;
    return y;
  };
}

Twist TwistFilter::smoothTwist(const Twist& twist) const
{
  static auto lp_lx = lowpass_filter();
  static auto lp_az = lowpass_filter();
  // apply lowpass filter to linear_x / angular_z
  Twist twist_out;
  twist_out.lx = lp_lx(twist.lx, config_.lowpass_gain_linear_x);
  twist_out.az = lp_az(twist.az, config_.lowpass_gain_angular_z);
  return twist_out;
}

这里用了lambda函数，也可以写成如下形式：

static double twist_out=twist;
twist_out.lx = config_.lowpass_gain_linear_x*twist_out.lx+(1-config_.lowpass_gain_linear_x)*twist.lx;
twist_out.az = config_.lowpass_gain_angular_z*twist_out.az+(1-config_.lowpass_gain_angular_z)*twist.az;

对于ctrlCmdCallback的cmd.linear_velocity,cmd.steering_angle处理类似。
总的来说，twist_filter是对线速度和角速度进行一个满足车辆运动学的检验，不能超过给定横向加速度阈值，否则做减速处理。