URL:navigation/Tutorials/RobotSetup/Odom - ROS Wiki
通过ROS发布里程计信息
导航堆栈使用 tf 来确定机器人在世界中的位置,并将传感器数据与静态地图相关联。但是,tf 不提供有关机器人速度的任何信息。因此,导航堆栈要求任何里程计源通过包含速度信息的 ROS 发布转换和 nav_msgs/Odometry消息。本教程解释了 nav_msgs/Odiometry 消息,并提供了分别通过 ROS 和 tf 发布消息和转换的示例代码。
The nav_mags/Odometry Message
nav_msgs/Odometry消息存储机器人在自由空间中的位置和速度的估计值,此消息中的姿态对应于机器人在里程坐标系中的估计位置,以及用于确定该姿态估计的可选协方差。此消息中的twist对应于机器人在child frame中的速度,通常是移动底座的坐标系,以及用于确定该速度估计的可选协方差。
使用tf发布里程计变换
正如变换配置教程中所讨论的,“tf”软件库负责管理变换树中与机器人相关的坐标系之间的关系。因此,任何里程计源都必须发布有关其管理的坐标系的信息。下面的代码假定您具备 tf 的基本知识,阅读转换配置教程就足够了。
代码
在本节中,我们将编写一些示例代码,用于在 ROS 上发布nav_msgs/里程计消息,并使用 tf 对一个只绕圈行驶的假机器人进行转换。我们将首先完整地展示代码,下面将逐个解释
需要的软件包 :tf nav_msgs
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
/*
包含头文件
*/
int main(int argc, char** argv){
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
//创建一个Publisher对象odom_pub 将odom话题作为发布的话题 50是消息队列的大小
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
//创建一个TransformBroadcaster对象odom_broadcaster用于发送TF消息
tf::TransformBroadcaster odom_broadcaster;
//定义一些变量 位置x,y,th 和速度 vx,vy,vth
//假设机器人最初从odom坐标系原点开始
double x = 0.0;
double y = 0.0;
double th = 0.0;
//在这里,我们将设置一些速度,
// 这些速度将导致“base_link”帧在“odom”帧中以 x 方向的 0.1m/s 速率移动,
// 在 y 方向上以 -0.1m/s 的速度移动,在 th 方向上以 0.1rad/s 的速度移动。
// 这或多或少会导致我们的假机器人绕圈行驶
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
//获取当前时间和上一次时间
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
ros::Rate r(1.0);
while(n.ok()){
ros::spinOnce(); // check for incoming messages
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
//获取时间差
double dt = (current_time - last_time).toSec();
//计算机器人在xy方向上的位移
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
//计算机器人在角度上的位移
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
//将机器人的姿态yaw用四元数表示 并创建一个geometry_msgs::TransformStamped类型的变量
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//odom_trans 用于发布机器人的坐标变换信息
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
//参考坐标系设置为odom
odom_trans.header.frame_id = "odom";
//子坐标系设置为base_link
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//机器人在x轴上的平移量
odom.pose.pose.position.x = x;
//
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
//旋转信息
odom.pose.pose.orientation = odom_quat;
//
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
//publish the message
odom_pub.publish(odom);
last_time = current_time;
r.sleep();
}
}
,op
452



被折叠的 条评论
为什么被折叠?



