基于ROS发布里程计信息

这篇具有很好参考价值的文章主要介绍了基于ROS发布里程计信息。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

参考文档: navigationTutorialsRobotSetupOdom
参考博客:
(1)ROS机器人里程计模型
(2)ROS里程计消息nav_msgs/Odometry的可视化方法

1 常用坐标系系统模型

世界坐标系是描述机器人全局信息的坐标系;机器人坐标系是描述机器人自身信息的坐标系;传感器坐标系是描述传感器信息的坐标系。如下图所示坐标系之间的关系。
基于ROS发布里程计信息,ROS,学习,c++,linux,里程计

世界坐标系是固定不变的,机器人坐标系和传感器坐标系是在世界坐标系下描述的。机器人坐标系和传感器坐标系原点重合但是存在一定的角度,不同的机器人坐标系关系是不同的。当我们使用传感器数据时,这些坐标系间的关系就是我们变换矩阵的参数,因为传感器的数据必定是要变换到机器人坐标系或者世界坐标系中使用的

2 移动机器人位姿模型

移动机器人的位姿模型就是机器人在世界坐标系下的状态。常用随机变量 X t = ( x t , y t , θ t ) X_t =(x_t ,y_t ,θ_t ) Xt=(xt,yt,θt)来描述 t t t 时刻的机器人在世界坐标系下的状态,简称位姿
基于ROS发布里程计信息,ROS,学习,c++,linux,里程计

3 移动机器人里程计的计算

移动机器人的里程计就是机器人每时每刻在世界坐标系下位姿状态
里程计的计算是指以机器人上电时刻为世界坐标系的起点(机器人的航向角是世界坐标系X正方向)开始累积计算任意时刻机器人在世界坐标系下的位姿。通常计算里程计方法是速度积分推算:通过左右电机的编码器测得机器人的左右轮的速度VL和VR,在一个短的时刻△t内,认为机器人是匀速运动,并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量,然后将增量进行累加处理,关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计。
基于ROS发布里程计信息,ROS,学习,c++,linux,里程计
基于ROS发布里程计信息,ROS,学习,c++,linux,里程计
t a n ( δ ) = L R tan(\delta)=\frac{L}{R} tan(δ)=RL(1)
v = w R v=wR v=wR(2)
由(1)(2)得
w = v R = v t a n ( δ ) L w=\frac{v}{R}=\frac{vtan(\delta)}{L} w=Rv=Lvtan(δ)
θ = w ∗ d t \theta=w*dt θ=wdt

  //在给定机器人速度的情况下,以一种典型的方式计算里程计
  31     double dt = (current_time - last_time).toSec();
  32     double delta_x = v * cos(th) * dt;
  33     double delta_y = v * sin(th) * dt;
  34     double delta_th = (v * tan(steer) / wheel_base) * dt;
  35
  36     x += delta_x;
  37     y += delta_y;
  38     th += delta_th;

4 示例代码

示例代码,用于通过ROS发布。nav_msgs/Odometry消息,并使用tf对只在圆圈中行驶的机器人进行转换。我们将首先展示整个代码,并在下面进行逐段解释。

   1 #include <ros/ros.h>
   2 #include <tf/transform_broadcaster.h>
   3 #include <nav_msgs/Odometry.h>
   4 
   5 int main(int argc, char** argv){
   6   ros::init(argc, argv, "odometry_publisher");
   7 
   8   ros::NodeHandle n;
   9   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  10   tf::TransformBroadcaster odom_broadcaster;
  11 
  12   double x = 0.0;
  13   double y = 0.0;
  14   double th = 0.0;
  15 
  16   double vx = 0.1;
  17   double vy = -0.1;
  18   double vth = 0.1;
  19 
  20   ros::Time current_time, last_time;
  21   current_time = ros::Time::now();
  22   last_time = ros::Time::now();
  23 
  24   ros::Rate r(1.0);
  25   while(n.ok()){
  26 
  27     ros::spinOnce();               // 检查传入消息
  28     current_time = ros::Time::now();
  29 
  30     //在给定机器人速度的情况下,以一种典型的方式计算里程计
  31     double dt = (current_time - last_time).toSec();
  32     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  33     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  34     double delta_th = vth * dt;
  35 
  36     x += delta_x;
  37     y += delta_y;
  38     th += delta_th;
  39 
  40     //由于所有里程计都是6DOF,我们需要一个由yaw创建的四元数
  41     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
  42 
  43     // 首先,我们将通过tf发布转换
  44     geometry_msgs::TransformStamped odom_trans;
  45     odom_trans.header.stamp = current_time;
  46     odom_trans.header.frame_id = "odom";
  47     odom_trans.child_frame_id = "base_link";
  48 
  49     odom_trans.transform.translation.x = x;
  50     odom_trans.transform.translation.y = y;
  51     odom_trans.transform.translation.z = 0.0;
  52     odom_trans.transform.rotation = odom_quat;
  53 
  54     // 发送转换
  55     odom_broadcaster.sendTransform(odom_trans);
  56 
  57     //接下来,我们将通过ROS发布里程计消息
  58     nav_msgs::Odometry odom;
  59     odom.header.stamp = current_time;
  60     odom.header.frame_id = "odom";
  61 
  62     //设置位置
  63     odom.pose.pose.position.x = x;
  64     odom.pose.pose.position.y = y;
  65     odom.pose.pose.position.z = 0.0;
  66     odom.pose.pose.orientation = odom_quat;
  67 
  68     //设置速度
  69     odom.child_frame_id = "base_link";
  70     odom.twist.twist.linear.x = vx;
  71     odom.twist.twist.linear.y = vy;
  72     odom.twist.twist.angular.z = vth;
  73 
  74     //发布消息
  75     odom_pub.publish(odom);
  76 
  77     last_time = current_time;
  78     r.sleep();
  79   }
  80 }

(1)我们需要实现“odom”参考系到“base_link”参考系的变换,以及nav_msgs/Odometry消息的发布,所以首先需要包含相关的头文件。

#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry>

(2)定义一个消息发布者来发布“odom”消息,在定义一个tf广播,来发布tf变换信息

ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;

(3)我们假设机器人最初从“odom”坐标系的原点开始。

double x = 0.0;
double y = 0.0;
double th = 0.0;

(4)我们设置机器人的默认前进速度,让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s,Y轴速度-0.1m/s,角速度0.1rad/s的状态移动,这种状态下,可以让机器人保持圆周运动。

double vx = 0.1;
double vy = -0.1;
double vth = 0.1;

(5)在这个例子中,我们将以1Hz的速率发布里程计信息,以便于内省,大多数系统都希望以更高的速率发布里程计。

ros::Rate r(1.0);

(6)使用我们设置的速度信息,来计算并更新里程计的信息,包括单位时间内机器人在x轴、y轴的坐标变化和角度的变化。在实际系统中,需要更具里程计的实际信息进行更新。

  31     double dt = (current_time - last_time).toSec();
  32     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  33     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  34     double delta_th = vth * dt;
  35 
  36     x += delta_x;
  37     y += delta_y;
  38     th += delta_th;

(7)为了兼容二维和三维的功能包,让消息结构更加通用,里程计的偏航角需要转换成四元数才能发布,辛运的是,ROS为我们提供了偏航角与四元数相互转换的功能。

  41    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th)

(8) 创建一个tf发布需要使用的TransformStamped类型消息,然后根据消息结构填充当前的时间戳、参考系id、子参考系id,注意两个参考系的id必须要是“odom”和“base_link”。

  44     geometry_msgs::TransformStamped odom_trans;
  45     odom_trans.header.stamp = current_time;
  46     odom_trans.header.frame_id = "odom";
  47     odom_trans.child_frame_id = "base_link";

(9)在这里,我们从里程计数据中填写转换消息,然后使用TransformBroadcaster发送转换。

  49     odom_trans.transform.translation.x = x;
  50     odom_trans.transform.translation.y = y;
  51     odom_trans.transform.translation.z = 0.0;
  52     odom_trans.transform.rotation = odom_quat;
  53 
  54     //send the transform
  55     odom_broadcaster.sendTransform(odom_trans);

(10)别忘了,我们还要发布nav_msgs/Odometry消息,让导航包获取机器人的速度。创建消息变量,然后填充时间戳。

  57     //next, we'll publish the odometry message over ROS
  58     nav_msgs::Odometry odom;
  59     odom.header.stamp = current_time;
  60     odom.header.frame_id = "odom";

(11) 填充机器人的位置、速度,然后发布消息。我们将消息的child_frame_id设置为“base_link”帧,因为这是我们发送速度信息的坐标帧。

  62     //set the position
  63     odom.pose.pose.position.x = x;
  64     odom.pose.pose.position.y = y;
  65     odom.pose.pose.position.z = 0.0;
  66     odom.pose.pose.orientation = odom_quat;
  67 
  68     //set the velocity
  69     odom.child_frame_id = "base_link";
  70     odom.twist.twist.linear.x = vx;
  71     odom.twist.twist.linear.y = vy;
  72     odom.twist.twist.angular.z = vth;

5 ROS里程计消息nav_msgs/Odometry的可视化方法

参考大佬的博客:ROS里程计消息nav_msgs/Odometry的可视化方法
基于ROS发布里程计信息,ROS,学习,c++,linux,里程计里程计消息中的pose包含了位置pose.position和姿态pose.orientation
可视化方法
(1)在一个节点中订阅发布的里程计话题消息nav_msgs/Odometry
(2)创建geometry_msgs::PoseStamped对象接收里程计的位姿。
(3)创建nav_msgs/Path对象作为容器,将赋值后的对象push_back进nav_msgs/Path中并发布。
然后即可在rviz中订阅包含nav_msgs/Path的话题并可视化轨迹。
(1)编写消息收发节点文件path_3d.cpp

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
 
nav_msgs::Path  path; // 创建nav_msgs/Path对象
ros::Publisher  path_pub;
 
void pathCallback(const nav_msgs::Odometry::ConstPtr& odom_3d)
{
    geometry_msgs::PoseStamped position_3d;
    position_3d.pose.position.x = odom_3d->pose.pose.position.x; 
    position_3d.pose.position.y = odom_3d->pose.pose.position.y; 
    position_3d.pose.position.z = odom_3d->pose.pose.position.z;
    position_3d.pose.orientation = odom_3d->pose.pose.orientation;
 
 
    position_3d.header.stamp = odom_3d->header.stamp;
    position_3d.header.frame_id = "map";
 
    path.poses.push_back(position_3d);
    path.header.stamp = position_3d.header.stamp;
    path.header.frame_id = "map";
    path_pub.publish(path);
  
    std::cout << odom_3d -> header.stamp << ' ' << odom_3d->pose.pose.position.x << ' ' << odom_3d->pose.pose.position.y << ' ' << odom_3d->pose.pose.position.z << std::endl;
}
 
int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
 
    path_pub = ph.advertise<nav_msgs::Path>("odom3d_path", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/odometry_3d", 10, pathCallback);  //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名,别忘了修改
    
    ros::Rate loop_rate(1000);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

(2)CMakeLists.txt如下

cmake_minimum_required(VERSION 2.8.3)
project(path_3d)
 
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
 
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  message_generation
)
 
## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   geometry_msgs   std_msgs
 )
 
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES path_3d
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
  DEPENDS system_lib
)
 
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)
 
add_executable(path_3d src/path_3d.cpp) #${PROJECT_NAME}_node
target_link_libraries(path_3d ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node
add_dependencies(path_3d beginner_tutorials_generate_messages_cpp) #path_3d_node

不足之处望指正文章来源地址https://www.toymoban.com/news/detail-720436.html

到了这里,关于基于ROS发布里程计信息的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • SLAM-VIO视觉惯性里程计

    VIO(visual-inertial odometry)即视觉惯性里程计,有时也叫视觉惯性系统(VINS,visual-inertial system),是融合相机和IMU数据实现SLAM的算法,根据融合框架的区别又分为紧耦合和松耦合,松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块,将每个模块的输出结果进行融合

    2024年02月11日
    浏览(39)
  • IMU惯性里程计解算(附代码实现)

    一、系统概述 IMU是机器人常用的传感器之一,IMU对机器人的定位功能实现非常重要,其优点在于是内源传感器对外部环境变化不明显,输出频率高,缺点在于存在累积误差。本文主要记录一下在机器人定位中对IMU的使用和对惯性导航里程计的理解和实现。 本文代码主要依赖

    2023年04月15日
    浏览(44)
  • 经典文献阅读之--VoxelMap(体素激光里程计)

    作为激光里程计,常用的方法一般是特征点法或者体素法,最近Mars实验室发表了一篇文章《Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry》,同时还开源了代码在Github上。文中为雷达里程计提出了一种高效的概率自适应体素建图方法。地图是体素的集合,

    2024年02月16日
    浏览(41)
  • 机器人运动学——轮速里程计(SLAM)

    目录 一、机器人运动学分析 二、阿克曼小车  2.1运动学分析   想要让机器人运动, 除了提供目标速度还不够, 需要将机器人的目标速度转换每个电机实际的目标速度, 最终根据电机的目标速度对电机的控制实现对机器人的控制。 机器人的目标速度转换成电机的目标速度这

    2024年02月16日
    浏览(41)
  • 激光雷达数据和里程计数据的时间同步方法汇总

    激光雷达(LIDAR)和里程计数据的时间同步,可以采用多种方法,每种方法都有其适用场景和优势。以下是一些常用方法的汇总: 简单遍历同步 : 对两个数据流进行遍历,寻找时间戳最接近的数据对。 适用于数据量小的场景,实现简单。 双缓冲队列 : 为里程计和激光雷达

    2024年01月20日
    浏览(38)
  • 【机器人模拟-02】 模拟移动机器人设置里程计

            在本教程中,我将向您展示如何设置移动机器人的测程。本教程是“机器人模拟”指南中的第二个教程。测量位移是仿真中的重要内容,设置测程的官方教程在此页面上,但我将逐步引导您完成整个过程。         您可以在此处获取此项目的完整代码。让我们

    2024年02月16日
    浏览(48)
  • 视觉SLAM14讲笔记-第7讲-视觉里程计2

    直接法的引出 直接法是视觉里程计另一个主要分支,它与特征点法有很大的不同。 使用特征点法估计相机运动时,我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置,通过 最小化重投影误差 来优化相机运动。 相对地,在直接法中,我们并不需要知

    2024年02月09日
    浏览(37)
  • 自动驾驶地面车辆的雷达里程计:方法与数据集综述

    Nader J. Abu-Alrub, Nathir A. Rawashdeh, Senior Member, IEEE         摘要: 雷达里程计在过去十年中受到越来越多的关注。它是在不利条件下进行机器人状态估计的最佳解决方案之一,其他内部感知和外部感知传感器可能在这些条件下无法胜任。雷达被广泛采用,对天气和光照条件具

    2024年02月15日
    浏览(48)
  • SLAM和里程计评估工具——evo使用方法全解

            本帖的主要内容是整理evo的使用方法及各种命令,不含安装步骤及过程,还未安装的请移步其他博主。         evo目前支持的公开数据集格式有: TUM、KITTI、EuRoC 以及 ROS bagfile 。如果使用的数据集格式为这些中的某一种,那么无须额外的数据格式处理,就可以

    2024年02月08日
    浏览(47)
  • 如何使用imu和轮速里程计融合定位?代码怎么写

    使用IMU和轮速里程计融合定位的一般步骤如下: 将IMU和轮速里程计的数据预处理成需要的形式。 使用一种预测滤波器(例如卡尔曼滤波器或高斯滤波器)来预测机器人的位置和姿态。 使用轮速里程计的测量来校正预测的位置和姿态。 使用IMU的测量来校正预测的姿态。 下面是使

    2024年02月16日
    浏览(39)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包