ROS里程计模块调用（基于松林小车）

一、ROS里程计

ros里程计可以调用机器人传感器的信息，获取机器人速度和位姿的信息。

关于速度用Twist来获取消息，对于位置用pose对象消息来获取。代码只使用了订阅者，因为信息已经被里程计模块发布出来了。

里面无法直接得到距离或者位移信息，需要通过速度或者位姿结算，这里用积分的方式可以获取机器人移动的距离，其中写了一个消除静态误差的方法，实现的代码如下：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>//包含在nav_msgs包中的里程计头文件
// 加权平均误差
double mean_loss = 0.0;
// 加权平均参数
double lamba = 0.5;
double ds_max = 0.0;
// 测试得到的静止时的 delta_s 最大误差值
double ds_loss_max = 0.001;
//获取参数初始化
double x = 0.0;
double y = 0.0;
double s = 0.0;
//double th = 0.0;

double vx = 0.0;
double vy = 0.0;
double vth = 0.0;
//获取时间，ros中自带的包
ros::Time current_time, last_time;
//定义一个回调函数，后面在运行订阅者的时候会一直调用这个回调函数
void disCallback(const nav_msgs::Odometry::ConstPtr &msg)//找到里程计中的msg，用一个常量指针去读取
{

    ros::Rate r(1.0);//也是调用时间的操作

    ros::spinOnce(); // check for incoming messages
    current_time = ros::Time::now();
//获取信息里面的x和y方向的速度
    vx = msg->twist.twist.linear.x;