yocs_velocity_smoother速度平滑库知识

最新推荐文章于 2024-04-24 13:40:57 发布

原创最新推荐文章于 2024-04-24 13:40:57 发布

· 1.6k 阅读

3 ·

版权

文章标签：

#c++

ROS 专栏收录该内容

9 篇文章

订阅专栏

一.C++

（1）nth_element()用法

头文件：#include<algorithm>

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<iostream>
#include<algorithm>
#include<vector>
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<<a[i]<<" ";
	}
	cout<<endl;
	cout<<"a中第3小："<<a[2]<<endl;
	
	 nth_element(a,a+2,a+5,cmp);
	for(int i=0;i<5;i++)
	{
		cout<<a[i]<<" ";
	}
	cout<<endl;
    	cout<<"a中第3大为："<<a[2]<<endl;
	
	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() 函数：

容器支持的迭代器类型必须为随机访问迭代器。这意味着，nth_element() 函数只适用于 array、vector、deque 这 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<memory>

.............
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 <ros/ros.h>
#include <turtlesim/Pose.h>

//单个参数为：消息类型为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 <ros/ros.h>
#include <turtlesim/Pose.h>

//三个参数：常量指针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<turtlesim::Pose>("/turtle1/pose_one", 10, callback_one);
    //回调函数为单个参数时，这里直接使用callback，传递的单个参数为：接收话题名为/turtle1/pose_one中的内容；
    
    ros::Subscriber pose_sub = n.subscribe<turtlesim::Pose>("/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<yocs_velocity_smoother::paramsConfig>(nh);
    dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);
}

参考：https://blog.youkuaiyun.com/weixin_46181372/article/details/110951454

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

归一化确定最大速度增量

速度增量： $\Delta v = v_{current} - v_{front}$

$A = (\Delta v,\Delta w) \gg (\frac{\left | \Delta v \right |}{\sqrt{\Delta v^{2}+\Delta w^{2}}}, \frac{\left | \Delta w\right |}{\sqrt{\Delta v^{2}+\Delta w^{2}}})$

$B = (\Delta v_{max},\Delta w_{max}) \gg (\frac{\left | \Delta v_{max} \right |}{\sqrt{\Delta v_{max}^{2}+\Delta w_{max}^{2}}}, \frac{\left | \Delta w_{max}\right |}{\sqrt{\Delta v_{max}^{2}+\Delta w_{max}^{2}}})$