yocs_velocity_smoother速度平滑库知识

一.C++

（1）nth_element()用法

头文件：#include

nth_element:在数组或容器中将第几大或小的元素放到该放的位置上。（默认第几小，可以用cmp自定义为第几大）

如：nth_element(a,a+6,a+10);  //在a中，找到 “第七小”将其放到a[6].

例如：

int array[5] = {2 4 6 8 10};

上面这个序列中第3小的是6，第4小的是8，经过nth_element（）函数之后可以把第n小的元素K移动到第n的位置，并且位于K之前的位置比K小，位于K字后的位置比K大，nth_element（）函数默认是升序排序，可以自定义降序排序。

#include
#include
#include
using namespace std;
int cmp(int a,int b)
{
	return a>b;
}
int main()
{
	int a[5]={2,4,6,8,10};
	
	nth_element(a,a+2,a+5);  //在a中，找到 第三小 
	for(int i=0;i<5;i++)
	{
		cout<" ";
	}
	cout<
	cout<<"a中第3小："<2]<
	
	 nth_element(a,a+2,a+5,cmp);
	for(int i=0;i<5;i++)
	{
		cout<" ";
	}
	cout<
    	cout<<"a中第3大为："<2]<
	
	return 0;
}

//排序规则采用默认的升序排序
void nth_element (RandomAccessIterator first,
                  RandomAccessIterator nth,
                  RandomAccessIterator last);
//排序规则为自定义的 comp 排序规则
void nth_element (RandomAccessIterator first,
                  RandomAccessIterator nth,
                  RandomAccessIterator last,
                  Compare comp);

其中，各个参数的含义如下：

• first 和 last：都是随机访问迭代器，[first, last) 用于指定该函数的作用范围（即要处理哪些数据）；
• nth：也是随机访问迭代器，其功能是令函数查找v_y_inc“第 nth 大”的元素，并将其移动到 nth 指向的位置；
• comp：用于自定义排序规则。

注意，鉴于 nth_element() 函数中各个参数的类型，其只能对普通数组或者部分容器进行排序。换句话说，只有普通数组和符合以下全部条件的容器，才能使用使用 nth_element() 函数：

1. 容器支持的迭代器类型必须为随机访问迭代器。这意味着，nth_element() 函数只适用于 array、vector、deque 这 3 个容器。
2. 当选用默认的升序排序规则时，容器中存储的元素类型必须支持 <小于运算符；同样，如果选用标准库提供的其它排序规则，元素类型也必须支持该规则底层实现所用的比较运算符；
3. nth_element() 函数在实现过程中，需要交换某些元素的存储位置。因此，如果容器中存储的是自定义的类对象，则该类的内部必须提供移动构造函数和移动赋值运算符。

 velocity_smoother_nodelet.hpp程序代码

 //返回vector的中位数
double median(std::vector<double> values) {
    // Return the median element of an doubles vector
    nth_element(values.begin(), values.begin() + values.size()/2, values.end());
    return values[values.size()/2];
  };

(2)C++智能指针成员函数reset

头文件：#include

.............
typedef boost::shared_ptr< ::geometry_msgs::Twist > TwistPtr;
typedef boost::shared_ptr< ::geometry_msgs::Twist const> TwistConstPtr;
..........
 
geometry_msgs::Twist   target_vel;
geometry_msgs::TwistPtr cmd_vel; 
 
//target_vel初始化
void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg)
{
    ......
    target_vel.linear.x  =  msg->linear.x;
    target_vel.linear.y  =  msg->linear.y;
    target_vel.angular.z =  msg->angular.z；
    ......
}
 
void VelocitySmoother::spin()
{
    ......
    cmd_vel.reset(new geometry_msgs::Twist(target_vel));
    ......
}

cmd_vel是智能指针对象（shared_ptr< ::geometry_msgs::Twist >）

 cmd_vel.reset(new geometry_msgs::Twist(target_vel));

令cmd_vel智能指针存放指针target_vel，也就是指向target_vel的空间，并且释放原来的空间。

(3)boost::thread

boost::thread*  worker_thread_;
......
worker_thread_ = new boost::thread(boost::bind(&VelocitySmoother::spin, this));

 创建一个线程

boost::thread(threadFun);

参数：可以是函数对象或者函数指针，并且该函数无参数，返回void类型。

（待补充）

(4）boost::bind()使用方法

int f(int a,int b)
{
    return a+ b;
}
 
int g(int a,int b, int c)
{
    return a + b + c;
}

boost::bind(f,1,2)可以产生无参数函数对象，f(1,2)；boost::bind(g,1,2,3)可以产生无参函数g(1,2,3)

其中boost::bind()中使用较为频繁的还有占位符：

如：boost::bind(f, _1, 5) (x) 中，_1就是一个占位符，其位于f函数形参的第一形参 int a 的位置，5位于第二形参 int b 的位置；

_1 表示（x）参数列表的第一个参数；所以，boost::bind(f, _1, 5) (x) 相当于 f(x ,5)。再看下面这些例子：

boost::bind(f, _2, _1)(x, y);   //相当于f(y,x)，即_2表示后面参数列表的第二个位置：y
boost::bind(g, _1, 9, _1)(x);  //相当于g(x, 9, x)
boost::bind(g, _3, _3, _3)(x, y, z);  //相当于g(z, z, z)

ROS中常用的boost::bind()

ROS编程过程中，有许多需要给回调函数传递多个参数的情况

回调函数只有一个参数

#include 
#include 
 
//单个参数为：消息类型为turtlesim::Pose的常量指针msg
void callback_one(const turtlesim::PoseConstPtr& msg)  
{
    float pose_x;
    pose_x = msg->x;
    ROS_INFO("x = [%f]",pose_x);  //打印出小乌龟所在的x坐标
}

回调函数有多个参数

#include 
#include 
 
//三个参数：常量指针msg、x、y
void callback_more(const turtlesim::PoseConstPtr& msg, int x, int y)  
{
    float pose_x;
    pose_x = msg->x;
    ROS_INFO("x = [%f]",pose_x);  //打印出小乌龟所在的x坐标
    ROS_INFO("input_x = [%i] ; input_y = [%i]", x, y);  //打印出输入的参数x、y
}

 主函数

int input_x = 1;
int input_y = 2;
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "test");
    ros::NodeHandle n;
    
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose_one", 10, callback_one);
    //回调函数为单个参数时，这里直接使用callback，传递的单个参数为：接收话题名为/turtle1/pose_one中的内容；
    
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose_more", 10, boost::bind(&callback_more, _1, input_x, input_y));    
    //这里回调函数为三个参数时，使用boost::bind(&callback, _1, input_x, input_y)，这里_1即为占位符，为subscriber接收到的/turtle1/pose_more话题中的内容占位；相当于callback(turtlesim::PoseConst& msg, input_x, input_y)
    
    ros::Rate loop_rate(1);
    int i=1;
    while(i<=3)
    {
        ros::spinOnce();
        loop_rate.sleep();
        i++;
    }
    return 0;
}

yocs_velocity_smoother.cpp

void VelocitySmoother::reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level)
  {
    ROS_INFO("Reconfigure request : %f %f %f %f %f",
             config.speed_lim_v_x, config.speed_lim_v_y, config.speed_lim_w, config.accel_lim_v_x, config.accel_lim_v_y, config.accel_lim_w, config.decel_factor);
 
    speed_lim_v_x  = config.speed_lim_v_x;
    speed_lim_v_y  = config.speed_lim_v_y;
    speed_lim_w  = config.speed_lim_w;
    accel_lim_v_x  = config.accel_lim_v_x;
    accel_lim_v_y  = config.accel_lim_v_y;
    accel_lim_w  = config.accel_lim_w;
    decel_factor = config.decel_factor;
    decel_lim_v_x  = decel_factor*accel_lim_v_x;
    decel_lim_v_y  = decel_factor*accel_lim_v_y;
    decel_lim_w  = decel_factor*accel_lim_w;
  }
 
bool VelocitySmoother::init(ros::NodeHandle& nh)
  {
    // Dynamic Reconfigure 占位符_1, _2相当回调函数reconfigCB的config和level参数变量
    //相当于reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level)
    dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2);
 
    dynamic_reconfigure_server = new dynamic_reconfigure::Server(nh);
    dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);
}

参考：https://blog.csdn.net/weixin_46181372/article/details/110951454

二.yocs_velocity_smoother.cpp速度平滑处理spin()函数理解

归一化确定最大速度增量

速度增量：Δv=vcurrent−vfront" role="presentation">Δv=vcurrent−vfront$\mathrm{\Delta }v=v_{current}-v_{front}$

A=(Δv,Δw)≫(|Δv|Δv2+Δw2,|Δw|Δv2+Δw2)" role="presentation">A=(Δv,Δw)≫(|Δv|Δv2+Δw2√,|Δw|Δv2+Δw2√)$A=(\mathrm{\Delta }v,\mathrm{\Delta }w)\gg (\frac{\left|\mathrm{\Delta }v\right|}{\sqrt{\mathrm{\Delta }{v}^2+\mathrm{\Delta }{w}^2}},\frac{\left|\mathrm{\Delta }w\right|}{\sqrt{\mathrm{\Delta }{v}^2+\mathrm{\Delta }{w}^2}})$

B=(Δvmax,Δwmax)≫(|Δvmax|Δvmax2+Δwmax2,|Δwmax|Δvmax2+Δwmax2)" role="presentation">B=(Δvmax,Δwmax)≫(|Δvmax|Δv2max+Δw2max√,|Δwmax|Δv2max+Δw2max√)$B=(\mathrm{\Delta }{v}_{max},\mathrm{\Delta }{w}_{max})\gg (\frac{\left|\mathrm{\Delta }{v}_{max}\right|}{\sqrt{\mathrm{\Delta }{v}_{max}^2+\mathrm{\Delta }{w}_{max}^2}},\frac{\left|\mathrm{\Delta }{w}_{max}\right|}{\sqrt{\mathrm{\Delta }{v}_{max}^2+\mathrm{\Delta }{w}_{max}^2}})$

夹角

θ=arctan(ByBx)−arctan(AyAx)" role="presentation">θ=arctan(ByBx)−arctan(AyAx)$\theta =arctan(\frac{By}{Bx})-arctan(\frac{Ay}{Ax})$

如果θ>0" role="presentation">θ>0$\theta >0$

ByBx−AyAx>0→ByBx>AyAx→transfromBy>AyAx∗Bx" role="presentation">ByBx−AyAx>0→ByBx>AyAx→transfromBy>AyAx∗Bx$\frac{By}{Bx}-\frac{Ay}{Ax}>0\to \frac{By}{Bx}>\frac{Ay}{Ax}\stackrel{transfrom}{\to }By>\frac{Ay}{Ax}\ast Bx$

最大线速度增量在以速度增量为基下时小于给定最大角度增量:

Δwmax=|Δw||Δv|∗Δvmax" role="presentation" style="position: relative;">Δwmax=|Δw||Δv|∗Δvmax$\mathrm{\Delta }{w}_{max}=\frac{\left|\mathrm{\Delta }w\right|}{\left|\mathrm{\Delta }v\right|}\ast \mathrm{\Delta }{v}_{max}$

如果θ<0" role="presentation" style="position: relative;">θ<0$\theta <0$

ByBx−AyAx<0→ByBx<AyAx→transfromBx>AxAy∗By" role="presentation" style="position: relative;">ByBx−AyAx<0→ByBx<AyAx→transfromBx>AxAy∗By$\frac{By}{Bx}-\frac{Ay}{Ax}<0\to \frac{By}{Bx}<\frac{Ay}{Ax}\stackrel{transfrom}{\to }Bx>\frac{Ax}{Ay}\ast By$

同理

Δvmax=|Δv||Δw|∗Δwmax" role="presentation" style="position: relative;">Δvmax=|Δv||Δw|∗Δwmax$\mathrm{\Delta }{v}_{max}=\frac{\left|\mathrm{\Delta }v\right|}{\left|\mathrm{\Delta }w\right|}\ast \mathrm{\Delta }{w}_{max}$

输出速度：

|Δv|>Δvmax" role="presentation" style="position: relative;">|Δv|>Δvmax$\left|\mathrm{\Delta }v\right|>\mathrm{\Delta }{v}_{max}$

vout=vlast+sign(Δv)∗Δvmax" role="presentation" style="position: relative;">vout=vlast+sign(Δv)∗Δvmax$v_{out}=v_{last}+sign(\mathrm{\Delta }v)\ast \mathrm{\Delta }{v}_{max}$

|Δw|>Δwmax" role="presentation" style="position: relative;">|Δw|>Δwmax$\left|\mathrm{\Delta }w\right|>\mathrm{\Delta }{w}_{max}$

wout=wlast+sign(Δw)∗Δwmax" role="presentation" style="position: relative;">wout=wlast+sign(Δw)∗Δwmax$w_{out}=w_{last}+sign(\mathrm{\Delta }w)\ast \mathrm{\Delta }{w}_{max}$