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

一、ROS里程计

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

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

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

#include 
#include //包含在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;
    vy = msg->twist.twist.linear.y;

//计算一些积分
    double dt = (current_time - last_time).toSec();
    double delta_x = vx * dt;
    double delta_y = vy * dt;
    double delta_s = (sqrt(delta_x * delta_x + delta_y * delta_y));

//将积分结果累加
    x += delta_x;
    y += delta_y;
    s += delta_s;
    if(delta_s < ds_loss_max) {//判断是静止的还是运动的
        mean_loss = delta_s * lamba + mean_loss*(1-lamba);
    }
    // 消除误差
    delta_s -= mean_loss;//将静差delta_s减去
    x += delta_x;
    last_time = current_time;
    //打印输出这些变量
    ROS_INFO("vx = %0.6f", vx);
    ROS_INFO("vy = %0.6f", vy);
    ROS_INFO("x = %0.6f", x);
    ROS_INFO("y = %0.6f", y);
    ROS_INFO("s = %0.6f", s);
}

int main(int argc, char **argv) // 节点主函数
{
    ros::init(argc, argv, "speed_subscriber"); // 初始化节点名称
    ros::NodeHandle n;                         // 声明用于ROS系统和通信的节点句柄
    current_time = ros::Time::now();
    last_time = ros::Time::now();//再次申明时间变量
    ros::Subscriber dis_sub = n.subscribe("/odom", 100, disCallback);
    // 上面是创建一个订阅者，订阅里程计发送的msg
    //订阅者是dis_sub  订阅的是/odom中的消息，以100次的频率调用回调函数，进行消息获取。
    // tf::TransformBroadcaster odom_broadcaster;

    ros::spin();//等待系统相应
    return 0;
}
// compute odometry in a typical way given the velocities of the robot

代码中有详细的注释，希望对您有帮助。