//排序规则采用默认的升序排序
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 values) {// Return the median element of an doubles vectornth_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()函数理解

归一化确定最大速度增量

速度增量：

夹角

如果

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

如果

同理

输出速度：