【记录学习】本文对yujin_ocs/yocs_velocity_smoother/src/velocity_smoother_nodelet.cpp速度平滑源码进行了解读，初学ros，若有错误欢迎指正，或有其他见解欢迎评论，在学习ros的道路上一起成长哦。

速度平滑一般情况下以里程计反馈的速度为作为重要参考，再订阅导航规功能包发布的速度命令，先进行限幅，后进行加速度归一化平滑处理，最后得到较为平滑的速度指令

yocs_velocity_smoother安装配置与使用请参考：
https://blog.csdn.net/LW_12345/article/details/119541037?spm=1001.2014.3001.5501

思维导图

代码分析

相关文件：
velocity_smoother_nodelet.hpp
velocity_smoother_nodelet.cpp

主要函数：

VelocitySmoother::reconfigCB |获取配置文件参数
VelocitySmoother::velocityCB |处理订阅到需要平滑的速度信息
VelocitySmoother::odometryCB/VelocitySmoother::robotVelCB |处理订阅到的里程计/机器人速度信息，并选择使用哪个做为参考
VelocitySmoother::spin |对加速度归一化平滑处理

加速度归一化平滑原理及效果

① 归一化确定最大速度增量

归一化向量A,B
A=(∣Δv|Δv2+Δw2,∣Δw|Δv2+Δw2)A = (\frac{{|\Delta {\text{v|}}}}{{\sqrt {\Delta {{\text{v}}^2} + \Delta {{\text{w}}^2}} }},\frac{{|\Delta {\text{w|}}}}{{\sqrt {\Delta {{\text{v}}^2} + \Delta {{\text{w}}^2}} }}) A=(Δv2+Δw2∣Δv|,Δv2+Δw2∣Δw|)
B=(Δvmax⁡Δvmax⁡2+Δwmax⁡2,Δwmax⁡Δvmax⁡2+Δwmax⁡2)B = (\frac{{\Delta {{\text{v}}_{\max }}}}{{\sqrt {\Delta {{\text{v}}_{\max }}^2 + \Delta {{\text{w}}_{\max }}^2} }},\frac{{\Delta {{\text{w}}_{\max }}}}{{\sqrt {\Delta {{\text{v}}_{\max }}^2 + \Delta {{\text{w}}_{\max }}^2} }})B=(Δvmax2+Δwmax2Δvmax,Δvmax2+Δwmax2Δwmax)
向量夹角θ
θ= arctan⁡(AyAx)−arctan⁡(ByBx)\theta {\text{ = }}\arctan (\frac{{Ay}}{{Ax}}) - \arctan (\frac{{By}}{{Bx}})θ = arctan(AxAy)−arctan(BxBy)
若θ>0,即由上式得：
AyAx−ByBx>0⇒AyAx>ByBx⇒Bx>AxAyBy⇒Δvmax⁡>∣Δv|∣Δw|Δwmax⁡⇒\frac{{Ay}}{{Ax}} - \frac{{By}}{{Bx}} > 0 \Rightarrow \frac{{Ay}}{{Ax}} > \frac{{By}}{{Bx}} \Rightarrow Bx > \frac{{Ax}}{{Ay}}By \Rightarrow \Delta {{\text{v}}_{\max }} > \frac{{|\Delta {\text{v|}}}}{{|\Delta {\text{w|}}}}\Delta {{\text{w}}_{\max }} \Rightarrow AxAy−BxBy>0⇒AxAy>BxBy⇒Bx>AyAxBy⇒Δvmax>∣Δw|∣Δv|Δwmax⇒
最大线速度增量在以速度增量为基下时小于给定最大角度增量:
则令
Δvmax⁡=∣Δv|∣Δw|Δwmax⁡\Delta {{\text{v}}_{\max }} = \frac{{|\Delta {\text{v|}}}}{{|\Delta {\text{w|}}}}\Delta {{\text{w}}_{\max }}Δvmax=∣Δw|∣Δv|Δwmax
若θ<0,同理令：
Δwmax⁡=∣Δw|∣Δv|Δvmax⁡3\Delta {{\text{w}}_{\max }} = \frac{{|\Delta {\text{w|}}}}{{|\Delta {\text{v|}}}}\Delta {{\text{v}}_{\max }} {3}Δwmax=∣Δv|∣Δw|Δvmax3
如下图

注：红色向量是新的最大速度增量的向量。将给定的最大速度增量，换算为在以速度增量为基下的最大速度增量
猜想用途是：平滑前后的速度增量比不变，是为了保证车的轨迹在平滑前后不变，即只保证轨迹不变，但不保证速度（需要验证？？？）

② 输出速度

若∣△v| > △vmax⁡\ |\vartriangle {\text{v| > }}\vartriangle {{\text{v}}_{\max }} ∣△v| > △vmax
则：
v=vlast+sign(△v)×△vmax⁡v = {v_{last}} + sign(\vartriangle v) \times \vartriangle {{\text{v}}_{\max }}v=vlast+sign(△v)×△vmax
若∣△w| > △wmax⁡\ |\vartriangle {\text{w| > }}\vartriangle {{\text{w}}_{\max }} ∣△w| > △wmax
则：
w=wlast+sign(△w)×△wmax⁡w = {w_{last}} + sign(\vartriangle w) \times \vartriangle {{\text{w}}_{\max }}w=wlast+sign(△w)×△wmax
其他：不需要滤波

③ 结果展示

源码

链接：https://github.com/yujinrobot/yujin_ocs.git（branch：indigo）

/*** @file /src/velocity_smoother_nodelet.cpp** @brief Velocity smoother implementation.** License: BSD*   https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_velocity_smoother/LICENSE**/
/******************************************************************************* Includes*****************************************************************************/#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>#include <dynamic_reconfigure/server.h>
#include <yocs_velocity_smoother/paramsConfig.h>#include <ecl/threads/thread.hpp>#include "yocs_velocity_smoother/velocity_smoother_nodelet.hpp"/******************************************************************************* Preprocessing*****************************************************************************/#define PERIOD_RECORD_SIZE    5
#define ZERO_VEL_COMMAND      geometry_msgs::Twist();
#define IS_ZERO_VEOCITY(a)   ((a.linear.x == 0.0) && (a.angular.z == 0.0))/*****************************************************************************
** Namespaces
*****************************************************************************/namespace yocs_velocity_smoother {/*********************
** Implementation
**********************/VelocitySmoother::VelocitySmoother(const std::string &name)
: name(name)
, quiet(false)
, shutdown_req(false)
, input_active(false)
, pr_next(0)
, dynamic_reconfigure_server(NULL)
{};void VelocitySmoother::reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level)
{ROS_INFO("Reconfigure request : %f %f %f %f %f",config.speed_lim_v, config.speed_lim_w, config.accel_lim_v, config.accel_lim_w, config.decel_factor);speed_lim_v  = config.speed_lim_v;speed_lim_w  = config.speed_lim_w;accel_lim_v  = config.accel_lim_v;accel_lim_w  = config.accel_lim_w;decel_factor = config.decel_factor;decel_lim_v  = decel_factor*accel_lim_v;decel_lim_w  = decel_factor*accel_lim_w;
}void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg)
{// Estimate commands frequency; we do continuously as it can be very different depending on the// publisher type, and we don't want to impose extra constraints to keep this package flexibleif (period_record.size() < PERIOD_RECORD_SIZE){period_record.push_back((ros::Time::now() - last_cb_time).toSec());}else{period_record[pr_next] = (ros::Time::now() - last_cb_time).toSec();}pr_next++;pr_next %= period_record.size();last_cb_time = ros::Time::now();if (period_record.size() <= PERIOD_RECORD_SIZE/2){// wait until we have some values; make a reasonable assumption (10 Hz) meanwhilecb_avg_time = 0.1;}else{// enough; recalculate with the latest inputcb_avg_time = median(period_record);}input_active = true;// Bound speed with the maximum valuestarget_vel.linear.x  =msg->linear.x  > 0.0 ? std::min(msg->linear.x,  speed_lim_v) : std::max(msg->linear.x,  -speed_lim_v);target_vel.angular.z =msg->angular.z > 0.0 ? std::min(msg->angular.z, speed_lim_w) : std::max(msg->angular.z, -speed_lim_w);
}void VelocitySmoother::odometryCB(const nav_msgs::Odometry::ConstPtr& msg)
{if (robot_feedback == ODOMETRY)current_vel = msg->twist.twist;// ignore otherwise
}void VelocitySmoother::robotVelCB(const geometry_msgs::Twist::ConstPtr& msg)
{if (robot_feedback == COMMANDS)current_vel = *msg;// ignore otherwise
}void VelocitySmoother::spin()
{double period = 1.0/frequency;ros::Rate spin_rate(frequency);while (! shutdown_req && ros::ok()){if ((input_active == true) && (cb_avg_time > 0.0) &&((ros::Time::now() - last_cb_time).toSec() > std::min(3.0*cb_avg_time, 0.5))){// Velocity input no active anymore; normally last command is a zero-velocity one, but reassure// this, just in case something went wrong with our input, or he just forgot good manners...// Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands// The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that// several messages arrive with the same time and so lead to a zero medianinput_active = false;if (IS_ZERO_VEOCITY(target_vel) == false){ROS_WARN_STREAM("Velocity Smoother : input got inactive leaving us a non-zero target velocity ("<< target_vel.linear.x << ", " << target_vel.angular.z << "), zeroing...[" << name << "]");target_vel = ZERO_VEL_COMMAND;}}if ((robot_feedback != NONE) && (input_active == true) && (cb_avg_time > 0.0) &&(((ros::Time::now() - last_cb_time).toSec() > 5.0*cb_avg_time)     || // 5 missing msgs(std::abs(current_vel.linear.x  - last_cmd_vel.linear.x)  > 0.2) ||(std::abs(current_vel.angular.z - last_cmd_vel.angular.z) > 2.0))){// If the publisher has been inactive for a while, or if our current commanding differs a lot// from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead// TODO: current command/feedback difference thresholds are 진짜 arbitrary; they should somehow// be proportional to max v and w...// The one for angular velocity is very big because is it's less necessary (for example the// reactive controller will never make the robot spin) and because the gyro has a 15 ms delayif ( !quiet ) {// this condition can be unavoidable due to preemption of current velocity control on// velocity multiplexer so be quiet if we're instructed to do soROS_WARN_STREAM("Velocity Smoother : using robot velocity feedback " <<std::string(robot_feedback == ODOMETRY ? "odometry" : "end commands") <<" instead of last command: " <<(ros::Time::now() - last_cb_time).toSec() << ", " <<current_vel.linear.x  - last_cmd_vel.linear.x << ", " <<current_vel.angular.z - last_cmd_vel.angular.z << ", [" << name << "]");}last_cmd_vel = current_vel;}geometry_msgs::TwistPtr cmd_vel;if ((target_vel.linear.x  != last_cmd_vel.linear.x) ||(target_vel.angular.z != last_cmd_vel.angular.z)){// Try to reach target velocity ensuring that we don't exceed the acceleration limitscmd_vel.reset(new geometry_msgs::Twist(target_vel));double v_inc, w_inc, max_v_inc, max_w_inc;v_inc = target_vel.linear.x - last_cmd_vel.linear.x;if ((robot_feedback == ODOMETRY) && (current_vel.linear.x*target_vel.linear.x < 0.0)){// countermarch (on robots with significant inertia; requires odometry feedback to be detected)max_v_inc = decel_lim_v*period;}else{max_v_inc = ((v_inc*target_vel.linear.x > 0.0)?accel_lim_v:decel_lim_v)*period;}w_inc = target_vel.angular.z - last_cmd_vel.angular.z;if ((robot_feedback == ODOMETRY) && (current_vel.angular.z*target_vel.angular.z < 0.0)){// countermarch (on robots with significant inertia; requires odometry feedback to be detected)max_w_inc = decel_lim_w*period;}else{max_w_inc = ((w_inc*target_vel.angular.z > 0.0)?accel_lim_w:decel_lim_w)*period;}// Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment),// where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines// which velocity (v or w) must be overconstrained to keep the direction provided as commanddouble MA = sqrt(    v_inc *     v_inc +     w_inc *     w_inc);double MB = sqrt(max_v_inc * max_v_inc + max_w_inc * max_w_inc);double Av = std::abs(v_inc) / MA;double Aw = std::abs(w_inc) / MA;double Bv = max_v_inc / MB;double Bw = max_w_inc / MB;double theta = atan2(Bw, Bv) - atan2(Aw, Av);if (theta < 0){// overconstrain linear velocitymax_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);}else{// overconstrain angular velocitymax_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc);}if (std::abs(v_inc) > max_v_inc){// we must limit linear velocitycmd_vel->linear.x  = last_cmd_vel.linear.x  + sign(v_inc)*max_v_inc;}if (std::abs(w_inc) > max_w_inc){// we must limit angular velocitycmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc;}smooth_vel_pub.publish(cmd_vel);last_cmd_vel = *cmd_vel;}else if (input_active == true){// We already reached target velocity; just keep resending last command while input is activecmd_vel.reset(new geometry_msgs::Twist(last_cmd_vel));smooth_vel_pub.publish(cmd_vel);}spin_rate.sleep();}
}/*** Initialise from a nodelet's private nodehandle.* @param nh : private nodehandle* @return bool : success or failure*/
bool VelocitySmoother::init(ros::NodeHandle& nh)
{// Dynamic Reconfiguredynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2);dynamic_reconfigure_server = new dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>(nh);dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);// Optional parametersint feedback;nh.param("frequency",      frequency,     20.0);nh.param("quiet",          quiet,         quiet);nh.param("decel_factor",   decel_factor,   1.0);nh.param("robot_feedback", feedback, (int)NONE);if ((int(feedback) < NONE) || (int(feedback) > COMMANDS)){ROS_WARN("Invalid robot feedback type (%d). Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)",feedback);feedback = NONE;}robot_feedback = static_cast<RobotFeedbackType>(feedback);// Mandatory parametersif ((nh.getParam("speed_lim_v", speed_lim_v) == false) ||(nh.getParam("speed_lim_w", speed_lim_w) == false)){ROS_ERROR("Missing velocity limit parameter(s)");return false;}if ((nh.getParam("accel_lim_v", accel_lim_v) == false) ||(nh.getParam("accel_lim_w", accel_lim_w) == false)){ROS_ERROR("Missing acceleration limit parameter(s)");return false;}// Deceleration can be more aggressive, if necessarydecel_lim_v = decel_factor*accel_lim_v;decel_lim_w = decel_factor*accel_lim_w;// Publishers and subscribersodometry_sub    = nh.subscribe("odometry",      1, &VelocitySmoother::odometryCB, this);current_vel_sub = nh.subscribe("robot_cmd_vel", 1, &VelocitySmoother::robotVelCB, this);raw_in_vel_sub  = nh.subscribe("raw_cmd_vel",   1, &VelocitySmoother::velocityCB, this);smooth_vel_pub  = nh.advertise <geometry_msgs::Twist> ("smooth_cmd_vel", 1);return true;
}/*********************
** Nodelet
**********************/class VelocitySmootherNodelet : public nodelet::Nodelet
{public:VelocitySmootherNodelet()  { }~VelocitySmootherNodelet(){NODELET_DEBUG("Velocity Smoother : waiting for worker thread to finish...");vel_smoother_->shutdown();worker_thread_.join();}std::string unresolvedName(const std::string &name) const {size_t pos = name.find_last_of('/');return name.substr(pos + 1);}virtual void onInit(){ros::NodeHandle ph = getPrivateNodeHandle();std::string resolved_name = ph.getUnresolvedNamespace(); // this always returns like /robosem/goo_arm - why not unresolved?std::string name = unresolvedName(resolved_name); // unresolve it ourselvesNODELET_DEBUG_STREAM("Velocity Smoother : initialising nodelet...[" << name << "]");vel_smoother_.reset(new VelocitySmoother(name));if (vel_smoother_->init(ph)){NODELET_DEBUG_STREAM("Velocity Smoother : nodelet initialised [" << name << "]");worker_thread_.start(&VelocitySmoother::spin, *vel_smoother_);}else{NODELET_ERROR_STREAM("Velocity Smoother : nodelet initialisation failed [" << name << "]");}}private:boost::shared_ptr<VelocitySmoother> vel_smoother_;ecl::Thread                        worker_thread_;
};} // namespace yocs_velocity_smootherPLUGINLIB_EXPORT_CLASS(yocs_velocity_smoother::VelocitySmootherNodelet, nodelet::Nodelet);```

yujin_ocs/yocs_velocity_smoother速度平滑velocity_smoother_nodelet源码解读相关推荐

Ubuntu 16.04下Caffe-SSD的应用（四）——ssd_pascal.py源码解读
前言 caffe-ssd所有的训练时的参数,全部由ssd_pascal.py来定义,之后再去调用相关的脚本和函数,所以想要训练自己的数据,首先要明白ssd_pascal.py各个定义参数的大体意思. ...
Linux内核网络协议栈：udp数据包发送（源码解读）
<监视和调整Linux网络协议栈:接收数据> <监控和调整Linux网络协议栈的图解指南:接收数据> <Linux网络 - 数据包的接收过程> <Linux网 ...
yolov1-v5学习笔记及源码解读
目录深度学习网络分类评价指标原理 yolov1 yolov2 yolov3 yolov4 yolov5 源码解读(v3为例) 深度学习网络分类深度学习经典检测方法通常分为 two-stage ...
yocs_velocity_smoother速度平滑配置与使用
yocs_velocity_smoother速度平滑配置与使用简介安装配置文件运行结果参考简介 yocs_velocity_smoother速度平滑(滤波)是将base_move导航输出的 ...
php artisan code,源码解读 Laravel PHP artisan config:cache
再来一篇源码解读系列,其实包含本篇 config:cache 源码解读在内,这三篇的源码解读都是跟线上环境部署 Laravel 项目有关,因为我们通常会使用这三个 artisan 命令来提高项目的执行 ...
Eureka的限流算法类RateLimiter源码解读
Eureka的限流算法类RateLimiter是基于令牌桶算法来实现的,下面看一看令牌桶算法的原理: 对于很多应用场景来说,除了要求能够限制数据的平均传输速率外,还要求允许某种程度的突发传输.这时候漏 ...
Vite 源码解读系列（图文结合） —— 本地开发服务器篇
哈喽,很高兴你能点开这篇博客,本博客是针对 Vite 源码的解读系列文章,认真看完后相信你能对 Vite 的工作流程及原理有一个简单的了解. Vite 是一种新型的前端构建工具,能够显著提升前端开发体 ...
【赠书福利】掘金爆火小册同名《Spring Boot源码解读与原理剖析》正式出书了！...
关注我们丨文末赠书承载着作者的厚望,掘金爆火小册同名读物<Spring Boot源码解读与原理剖析>正式出书! 本书前身是掘金社区销量TOP的小册--<Spring Boot源码解 ...
噪声对比估计NCE (Noise-contrastive estimation)采样方法，提高训练速度，解决源码中正label个数必须相等问题
目录 1 目的 2 tensorflow源码解读 2.1函数的输入和输出 2.2 代码讲解 2.3 缺点 3 正label个数不一致解决方案 3.1 增加一个pad标签作为负label 4 参考 1 ...

yujin_ocs/yocs_velocity_smoother速度平滑velocity_smoother_nodelet源码解读