Autoware学习笔记waypoint_follower之twist_filter

twis_filter.launch文件

<!-- -->

    <!-- rosrun waypoint_follower twist_filter -->    
    "waypoint_follower" type="twist_filter" name="twist_filter" output="log">    
    </node>    
    "waypoint_follower" type="twist_gate" name="twist_gate" output="log">    
    </node>
</launch>

launch文件中同时启动了twist_filter和twist_gate两个节点。

twist_filter节点

twist_filter的主函数如下：

int main(int argc, char **argv)
{    
    ros::init(argc, argv, "twist_filter");
    
    ros::NodeHandle nh;    
    ros::NodeHandle private_nh("~");
    
    ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback);    
    ros::Subscriber config_sub = nh.subscribe("config/twist_filter", 10, configCallback);    
    g_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 1000);
    
    ros::spin();    
    return 0;
}

初始化ROS，指定节点名称为“twist_filter”。
定义了两个订阅者用来接收“twist_raw”和“config/twist_filter”话题，并调用回调函数处理。其中twist_raw话题由pure_pursuit节点发布（见前面的博客），config/twist_fiter来自于runtime的config。
也定义了一个发布者发布消息类型为的twist_cmd话题。
主函数的内容比较简单，来看一下两个回调函数：

void TwistCmdCallback(const geometry_msgs::TwistStampedConstPtr &msg)
{
  double v = msg->twist.linear.x;  
  double omega = msg->twist.angular.z;
 
  if(fabs(omega) < ERROR)
  {    
    g_twist_pub.publish(*msg);    
    return;  
  }
  
  double max_v = g_lateral_accel_limit / fabs(omega);
  geometry_msgs::TwistStamped tp;  
  tp.header = msg->header;
  double a = v * omega;  ROS_INFO("lateral accel = %lf", a);

  tp.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v : v;  
  tp.twist.angular.z = omega;
  static double lowpass_linear_x = 0;  
  static double lowpass_angular_z = 0;  
  lowpass_linear_x = g_lowpass_gain_linear_x * lowpass_linear_x + (1 - g_lowpass_gain_linear_x) * tp.twist.linear.x;
  lowpass_angular_z = g_lowpass_gain_angular_z * lowpass_angular_z + (1 - g_lowpass_gain_angular_z) *tp.twist.angular.z;
  tp.twist.linear.x = lowpass_linear_x;  
  tp.twist.angular.z = lowpass_angular_z;
  ROS_INFO("v: %f -> %f",v,tp.twist.linear.x);  
  g_twist_pub.publish(tp);
}

接受的消息中包含速度V和角速度Omega，首先判断角速度大小 是否接近于0，是的话则认为当前是直线行驶不考虑横向加速度，直接发布接收到的消息。
若角速度的值不接近零（非直线行驶），有如下参数设定

double g_lateral_accel_limit = 5.0;
double g_lowpass_gain_linear_x = 0.0;
double g_lowpass_gain_angular_z = 0.0;
constexpr double RADIUS_MAX = 9e10;
constexpr double ERROR = 1e-8;

根据给定的横向加速度限制g_lateral_accel_limit = 5，得到当前角速度时对应的最大行驶速度max_v。计算当前的横向加速度a = v * omega这些公式涉及到物理知识也比较容易有疑问的可以复习一下。
新建geometry_msgs::TwistStamped tp。
如果根据接受到的速度和角速度计算出的横向加速度a大于给定的限值，则tp的线速度赋值为max_v，否则赋值为接收的速度v。其角速度Omega就是接收的角速度。
接着对输出的速度和角速度进行一阶低通滤波处理。
一阶低通滤波的算法公式为：
　　　　　　　　　　　　　 　 Y(n)=αX(n) + (1-α)Y(n-1)
　　式中：α=滤波系数（通常较小）；X(n)=本次采样值；Y(n-1)=上次滤波输出值；Y(n)=本次滤波输出值。
一阶低通滤波法采用本次采样值与上次滤波输出值进行加权，得到有效滤波值，其输出对输入有反馈作用。

其中在另一个回调函数中配置了一些参数。

void configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr &config)
{  
  g_lateral_accel_limit = config->lateral_accel_limit;  
  ROS_INFO("g_lateral_accel_limit = %lf",g_lateral_accel_limit);  
  g_lowpass_gain_linear_x = config->lowpass_gain_linear_x;  
  ROS_INFO("lowpass_gain_linear_x = %lf",g_lowpass_gain_linear_x);  
  g_lowpass_gain_angular_z = config->lowpass_gain_angular_z;  
  ROS_INFO("lowpass_gain_angular_z = %lf",g_lowpass_gain_angular_z);
}

参数可以通过runtime manage配置，如下图示例。

最后把处理好的消息发布出去geometry_msgs::TwistStamped>("twist_cmd"）
twist_filter节点主要是对速度和角速度进行一个满足车辆运动学的前提下进行检验，不能超过给定的横向加速度阈值，否则进行减速处理。

往往和 twist_filter节点一同工作的还有twist_gate，后续补上。

由于水平有限以上内容仅是自己学习理解过程，各位读者朋友发现问题之处还请帮忙提出，不胜感激。

有问题还请指正，不胜感激。

共同交流，共同进步！