navigation（3）

#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();
    }
}