用 ROS 发布里程计（rotary encoders）信息
The navigation stack uses tf to determine the robot's location in the world and relate sensor data to a static map. However, the "tf" software library is responsible for managing the relationships between coordinate frames relevant to the robot in a transform tree. Therefore, any odometry source must publish information about the coordinate frame that it manages. tf does not provide any information about the velocity of the robot.
Because of this, the navigation stack requires that any odometry source publish both a transform and a nav_msgs/Odometry message over ROS that contains velocity information.
This tutorial explains the nav_msgs/Odometry message and provides example code for publishing both the message and transform over ROS and tf respectively.
ROS使用坐标变换包tf来求解机器人的位置以及将传感器数据与静态地图关联起来，然而
tf软件库主要负责一以tf坐标变换树的形式管理机器人的各种坐标系之间的关系， tf中没有提供
机器人的速度信息，因此，导航功能包要求里程计信息源在ROS上发布位姿的变换信息以及
包含速度信息的里程计nav_msgs/Odometry 消息。
本篇将介绍nav_msgs/Odometry消息，并且通过代码实现变换与消息在ROS和tf上的发布。
---------------------------------------------
The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
The pose in this message corresponds to the estimated position of the robot in the odometric frame along with an optional covariance for the certainty of that pose estimate. The twist in this message corresponds to the robot's velocity in the child frame, normally the coordinate frame of the mobile base, along with an optional covariance for the certainty of that velocity estimate.
nav_msgs/Odometry 消息存储了对机器人位置和速度的一个估计。
pose参数包含机器人在header.frame_id（里程计参考系）下的位置估算值，同时带有可选的表示位置估计确定性的协方差。
twist参数包含机器人在child_frame_id（子参考系，一般是机器人基坐标系）下的速度，同时带有可选的表示速度估计确定性的协方差。
// 驱动机器人画圆的整体代码例子
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
/* 我们需要发布 nav_msgs/Odometry 消息。tf/transform_broadcaster.h 用来发布坐标变换的
* 现“odom” 坐标系 到 “base_link” 参考系的变换，以差分式机器人为例 odom 是从差分轮编码器
* 得到的数值， base_link 是 机器人机体坐标系，机器人的轮子转多少圈与小车走多少米有个转
* 换关系。*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
/* 定义odom_pub 用来发布 名字为“odom”， 类型为nav_msgs/Odometry 的消息，
* 定义odom_broadcaster，用来发布从 "odom"坐标系 到 "base_link" 坐标系的 变换信息。*/
double x = 0.0;
double y = 0.0;
double th = 0.0;
/* 默认机器人的起始位置是odom参考系下的0点。*/
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
/* 我们设置机器人的base_link参考系在odom参考系下， 以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动，这种状态下，可以让机器人保持圆周运动。
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
ros::Rate r(1.0);
/* 使用1Hz的频率发布odom消息，当然，在实际系统中，往往需要更快的速度进行发布。*/
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();
// Time and Duration objects can also be turned into floating point seconds
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;
/* 使用我们设置的速度信息来计算并更新里程计的信息，机器人在x轴、y轴的坐标和角度。
* 在实际系统中，需要对速度进行积分。*/
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
/* 将里程计的偏航角转换成四元数，四元数效率高，这样使用二维和三维的功能包是一样的。*/
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
/*
Here we'll create a TransformStamped message that we will send out over tf. We want to publish the transform from the "odom" frame to the "base_link" frame at current_time. Therefore, we'll set the header of the message and the child_frame_id accordingly, making sure to use "odom" as the parent coordinate frame and "base_link" as the child coordinate frame.
*/
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);
/*
在此处我们发布了当前最新base_link坐标系与odem坐标系之间的变换。
*/
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
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;
/*
Here we fill in the transform message from our odometry data, and then send the transform using our TransformBroadcaster.
*/
//set the velocity
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);
/*
我们发布nav_msgs/Odometry消息(odom)，从而导航包能从中获取机器人的速度。
我们设置child_frame_id为"base_link"坐标系，因为我们计算的速度才是base_link 坐标系下的速度。
*/
last_time = current_time;
r.sleep();
}
}
/////////// others //////////////////
# ------------- TransformStamped.msg --------------------------------------
# This expresses a transform from coordinate frame header.frame_id
# to the coordinate frame child_frame_id
#
# This message is mostly used by the
# tf package.
# See its documentation for more information.
Header header
string child_frame_id # the frame id of the child frame
Transform transform
# -----------------------Transform.msg ------------------------------
# This represents the transform between two coordinate frames in free space.
Vector3 translation
Quaternion rotation
#--------------------- Quaternion.msg ----------------------
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
#--------------------- Odometry.msg ---------------------
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
# ------------- PoseWithCovariance.msg -----------------------
# This represents a pose in free space with uncertainty.
Pose pose
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance
#---------------- Pose.msg --------------
# A representation of pose in free space, composed of postion and orientation.
Point position
Quaternion orientation
# --------------- Point.msg -----------------------
# This contains the position of a point in free space
float64 x
float64 y
float64 z
# ---------------------TwistWithCovariance.msg -------------------------
# This expresses velocity in free space with uncertainty.
Twist twist
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance
-------------Twist.msg---------------------------------
# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 angular
============= rotary encoders 的缺点与局限 ======================
In navigation, odometry is the use of data from the movement of actuators to estimate change in position over time through devices such as rotary encoders to measure wheel rotations. While useful for many wheeled or tracked vehicles, traditional odometry techniques cannot be applied to mobile robots with non-standard locomotion methods, such as legged robots. In addition, odometry universally suffers from precision problems, since wheels tend to slip and slide on the floor creating a non-uniform distance traveled as compared to the wheel rotations. The error is compounded when the vehicle operates on non-smooth surfaces. Odometry readings become increasingly unreliable over time as these errors accumulate and compound over time.
Because of this, the navigation stack requires that any odometry source publish both a transform and a nav_msgs/Odometry message over ROS that contains velocity information.
This tutorial explains the nav_msgs/Odometry message and provides example code for publishing both the message and transform over ROS and tf respectively.
ROS使用坐标变换包tf来求解机器人的位置以及将传感器数据与静态地图关联起来，然而
tf软件库主要负责一以tf坐标变换树的形式管理机器人的各种坐标系之间的关系， tf中没有提供
机器人的速度信息，因此，导航功能包要求里程计信息源在ROS上发布位姿的变换信息以及
包含速度信息的里程计nav_msgs/Odometry 消息。
本篇将介绍nav_msgs/Odometry消息，并且通过代码实现变换与消息在ROS和tf上的发布。
---------------------------------------------
The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
The pose in this message corresponds to the estimated position of the robot in the odometric frame along with an optional covariance for the certainty of that pose estimate. The twist in this message corresponds to the robot's velocity in the child frame, normally the coordinate frame of the mobile base, along with an optional covariance for the certainty of that velocity estimate.
nav_msgs/Odometry 消息存储了对机器人位置和速度的一个估计。
pose参数包含机器人在header.frame_id（里程计参考系）下的位置估算值，同时带有可选的表示位置估计确定性的协方差。
twist参数包含机器人在child_frame_id（子参考系，一般是机器人基坐标系）下的速度，同时带有可选的表示速度估计确定性的协方差。
// 驱动机器人画圆的整体代码例子
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
/* 我们需要发布 nav_msgs/Odometry 消息。tf/transform_broadcaster.h 用来发布坐标变换的
* 现“odom” 坐标系 到 “base_link” 参考系的变换，以差分式机器人为例 odom 是从差分轮编码器
* 得到的数值， base_link 是 机器人机体坐标系，机器人的轮子转多少圈与小车走多少米有个转
* 换关系。*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
/* 定义odom_pub 用来发布 名字为“odom”， 类型为nav_msgs/Odometry 的消息，
* 定义odom_broadcaster，用来发布从 "odom"坐标系 到 "base_link" 坐标系的 变换信息。*/
double x = 0.0;
double y = 0.0;
double th = 0.0;
/* 默认机器人的起始位置是odom参考系下的0点。*/
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
/* 我们设置机器人的base_link参考系在odom参考系下， 以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动，这种状态下，可以让机器人保持圆周运动。
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
ros::Rate r(1.0);
/* 使用1Hz的频率发布odom消息，当然，在实际系统中，往往需要更快的速度进行发布。*/
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();
// Time and Duration objects can also be turned into floating point seconds
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;
/* 使用我们设置的速度信息来计算并更新里程计的信息，机器人在x轴、y轴的坐标和角度。
* 在实际系统中，需要对速度进行积分。*/
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
/* 将里程计的偏航角转换成四元数，四元数效率高，这样使用二维和三维的功能包是一样的。*/
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
/*
Here we'll create a TransformStamped message that we will send out over tf. We want to publish the transform from the "odom" frame to the "base_link" frame at current_time. Therefore, we'll set the header of the message and the child_frame_id accordingly, making sure to use "odom" as the parent coordinate frame and "base_link" as the child coordinate frame.
*/
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);
/*
在此处我们发布了当前最新base_link坐标系与odem坐标系之间的变换。
*/
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
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;
/*
Here we fill in the transform message from our odometry data, and then send the transform using our TransformBroadcaster.
*/
//set the velocity
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);
/*
我们发布nav_msgs/Odometry消息(odom)，从而导航包能从中获取机器人的速度。
我们设置child_frame_id为"base_link"坐标系，因为我们计算的速度才是base_link 坐标系下的速度。
*/
last_time = current_time;
r.sleep();
}
}
/////////// others //////////////////
# ------------- TransformStamped.msg --------------------------------------
# This expresses a transform from coordinate frame header.frame_id
# to the coordinate frame child_frame_id
#
# This message is mostly used by the
# tf package.
# See its documentation for more information.
Header header
string child_frame_id # the frame id of the child frame
Transform transform
# -----------------------Transform.msg ------------------------------
# This represents the transform between two coordinate frames in free space.
Vector3 translation
Quaternion rotation
#--------------------- Quaternion.msg ----------------------
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
#--------------------- Odometry.msg ---------------------
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
# ------------- PoseWithCovariance.msg -----------------------
# This represents a pose in free space with uncertainty.
Pose pose
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance
#---------------- Pose.msg --------------
# A representation of pose in free space, composed of postion and orientation.
Point position
Quaternion orientation
# --------------- Point.msg -----------------------
# This contains the position of a point in free space
float64 x
float64 y
float64 z
# ---------------------TwistWithCovariance.msg -------------------------
# This expresses velocity in free space with uncertainty.
Twist twist
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance
-------------Twist.msg---------------------------------
# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 angular
 |
============= rotary encoders 的缺点与局限 ======================
In navigation, odometry is the use of data from the movement of actuators to estimate change in position over time through devices such as rotary encoders to measure wheel rotations. While useful for many wheeled or tracked vehicles, traditional odometry techniques cannot be applied to mobile robots with non-standard locomotion methods, such as legged robots. In addition, odometry universally suffers from precision problems, since wheels tend to slip and slide on the floor creating a non-uniform distance traveled as compared to the wheel rotations. The error is compounded when the vehicle operates on non-smooth surfaces. Odometry readings become increasingly unreliable over time as these errors accumulate and compound over time.
还没人转发这篇日记
