【记录学习】本文对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|
Δ
v
2
+
Δ
w
2
,
∣
Δ
w|
Δ
v
2
+
Δ
w
2
)
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
=
(
Δ
v
max
Δ
v
max
2
+
Δ
w
max
2
,
Δ
w
max
Δ
v
max
2
+
Δ
w
max
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
(
A
y
A
x
)
−
arctan
(
B
y
B
x
)
\theta {\text{ = }}\arctan (\frac{{Ay}}{{Ax}}) - \arctan (\frac{{By}}{{Bx}})
θ = arctan(AxAy)−arctan(BxBy)
若θ>0,即由上式得:
A
y
A
x
−
B
y
B
x
>
0
⇒
A
y
A
x
>
B
y
B
x
⇒
B
x
>
A
x
A
y
B
y
⇒
Δ
v
max
>
∣
Δ
v|
∣
Δ
w|
Δ
w
max
⇒
\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⇒
最大线速度增量在以速度增量为基下时小于给定最大角度增量:
则令
Δ
v
max
=
∣
Δ
v|
∣
Δ
w|
Δ
w
max
\Delta {{\text{v}}_{\max }} = \frac{{|\Delta {\text{v|}}}}{{|\Delta {\text{w|}}}}\Delta {{\text{w}}_{\max }}
Δvmax=∣Δw|∣Δv|Δwmax
若θ<0,同理令:
Δ
w
max
=
∣
Δ
w|
∣
Δ
v|
Δ
v
max
3
\Delta {{\text{w}}_{\max }} = \frac{{|\Delta {\text{w|}}}}{{|\Delta {\text{v|}}}}\Delta {{\text{v}}_{\max }} {3}
Δwmax=∣Δv|∣Δw|Δvmax3
如下图
注:红色向量是新的最大速度增量的向量。将给定的最大速度增量,换算为在以速度增量为基下的最大速度增量
猜想用途是:平滑前后的速度增量比不变,是为了保证车的轨迹在平滑前后不变,即只保证轨迹不变,但不保证速度(需要验证???)
② 输出速度
若
∣
△
v| >
△
v
max
\ |\vartriangle {\text{v| > }}\vartriangle {{\text{v}}_{\max }}
∣△v| > △vmax
则:
v
=
v
l
a
s
t
+
s
i
g
n
(
△
v
)
×
△
v
max
v = {v_{last}} + sign(\vartriangle v) \times \vartriangle {{\text{v}}_{\max }}
v=vlast+sign(△v)×△vmax
若
∣
△
w| >
△
w
max
\ |\vartriangle {\text{w| > }}\vartriangle {{\text{w}}_{\max }}
∣△w| > △wmax
则:
w
=
w
l
a
s
t
+
s
i
g
n
(
△
w
)
×
△
w
max
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 flexible
if (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) meanwhile
cb_avg_time = 0.1;
}
else
{
// enough; recalculate with the latest input
cb_avg_time = median(period_record);
}
input_active = true;
// Bound speed with the maximum values
target_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 median
input_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 delay
if ( !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 so
ROS_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 limits
cmd_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 command
double 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 velocity
max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);
}
else
{
// overconstrain angular velocity
max_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 velocity
cmd_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 velocity
cmd_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 active
cmd_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 Reconfigure
dynamic_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 parameters
int 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 parameters
if ((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 necessary
decel_lim_v = decel_factor*accel_lim_v;
decel_lim_w = decel_factor*accel_lim_w;
// Publishers and subscribers
odometry_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 ourselves
NODELET_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_smoother
PLUGINLIB_EXPORT_CLASS(yocs_velocity_smoother::VelocitySmootherNodelet, nodelet::Nodelet);```