#include <ros/ros.h>
#include <tf/transform_broadcaster.h> //因为我们将同时发布一个从“odom”坐标系到“base_link”坐标系的转换和一个nav_msgs/里程表消息
#include <nav_msgs/Odometry.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n; //我们需要创建一个ros::Publisher和一个tf:: transform才能分别使用ros和tf发送消息。
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("Odom", 50);
tf::TransformBroadcaster odom_broadcaster;
double x = 0.0; //我们假设机器人从"odom"坐标系的原点开始。
double y = 0.0;
double th = 0.0;
double vx = 0.1; //设置速度,使“base_link”帧在“odom”帧中以x方向的0.1m/s、y方向的-0.1m/s和第一个方向的0.1rad/s移动。
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); //以1Hz的速率发布里程表信息,以方便内省,大多数系统都希望以更高的速率发布里程表。
while (n.ok) {
ros::spinOnce();
current_time = ros::Time::now();
//根据设置的恒定速度更新里程表信息。当然,一个真正的里程表系统会对计算出来的里程数进行积分。
double dt = (current_time - last_time).toSec();
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;
//从偏航值创建四元数
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
geometry_msgs::TransformStamped odom_trans; //创建一个转换后的消息,我们将通过tf发送出去
odom_trans.header.stamp = current_time; //我们想要在current_time将转换从“odom”帧发布到“base_link”帧
odom_trans.header.fram_id = "odom"; //确保使用“Odom”作为父坐标框架,使用“base_link”作为子坐标框架
odom_tarns.child_frame_id = "base_link";
//写来自里程表数据的转换消息,然后使用transform发送转换
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;
odom_broadcaster.sendTransform(odom_trans);
//发布一个nav_msgs/Odometry消息,以便导航堆栈可以从中获得速度信息
nav_msgs::Odometry odom;
odom.header.stamp = current_time; //将消息的标题设置为current_time和“odom”坐标框架。
odom.header.frame_id = "odom";
//这将用里程表数据填充消息并通过网络发送出去
//我们将消息的child_frame_id设置为“base_link”帧,因为这是正在发送速度信息的坐标系。
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = z;
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;
odom_pub.publisher(odom);
last_time = current_time;
r.sleep();
}
}