Auto Ware 代码解析系列-twist_filter节点

twist_filter节点主要完成对puresuit节点输出的汽车运动速度进行低通滤波处理，下面对一般的低滤波算法做说明：
电路中电荷Q与U 有关系式Q=CU.于是

参考他人资料

低通滤波算法如下：
Yn=a* Xn+(1-a) *Yn-1

式中 Xn——本次采样值

Yn-1——上次的滤波输出值;

a——滤波系数，其值通常远小于1;

Yn——本次滤波的输出值。
由上式可以看出，本次滤波的输出值主要取决于上次滤波的输出值(注意不是上次的采样值，这和加权平均滤波是有本质区别的)，本次采样值对滤波输出的贡献是比较小的，但多少有些修正作用，这种算法便模拟了具体有教大惯性的低通滤波器功能。滤波算法的截止频率可用以下式计算：

fL=a/2Pit pi为圆周率3.14…

式中 a——滤波系数;

， t——采样间隔时间;

例如：当t=0.5s(即每秒2次)，a=1/32时;

fL=(1/32)/(2*3.14*0.5)=0.01Hz

twist_filter节点代码如下：

#include <ros/ros.h>#include <geometry_msgs/TwistStamped.h>#include <iostream>#include "runtime_manager/ConfigTwistFilter.h"namespace {//Publisherros::Publisher g_twist_pub;double g_lateral_accel_limit = 5.0;//设置无人车的最大侧向加速度double g_lowpass_gain_linear_x = 0.0;//设置x方向线速度低通滤波器的增益double g_lowpass_gain_angular_z = 0.0;//设置角速度方向低通滤波器的增益constexpr double RADIUS_MAX = 9e10;//最大转弯半径constexpr double ERROR = 1e-8;//通过回调函数设置滤波器的参数void configCallback(const runtime_manager::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);}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 / 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);}} /// namespaceint 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;}