Rviz显示机器人运动轨迹

        在实际操作机器人进行移动的时候，虽然直接观察机器人本身是比较直观的，但是无法查看到机器人运动过程中比较细微的变化（比如方向的变化），那么就需要将机器人的运动轨迹用可视化软件Rviz显示出来以便于观察。这个功能是利用Rviz中的Path类型实现的，只需要将机器人的位置信息已nav_msgs/Path类型的消息的形式发布到ROS系统中，然后在Rviz上订阅该类型的消息就可以显示机器人的运动轨迹了

        下面了解一下nav_msgs/Path类型数据的数据结构：

Header header  
geometry_msgs/PoseStamped[] poses  
Header header  
    uint32 seq  
    time stamp  
    string frame_id  
geometry_msgs/PoseStamped[] poses  
    Header header  
        uint32 seq  
        time stamp  
        string frame_id  
    geometry_msgs/Pose pose  
        geometry_msgs/Point position  
            float64 x  
            float64 y  
            float64 z  
        geometry_msgs/Quaternion orientation  
            float64 x  
            float64 y  
            float64 z  
            float64 w

        从上面的数据结构中可以看出，每一个nav_msgs/Path的数据都是由头部-header和位姿数组-pose两部分组成的，其中位姿数组包含的信息就是机器人在移动过程中的每一帧的位姿信息。具体实现代码如下：

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<nav_msgs/Odometry.h>
#include<nav_msgs/Path.h>
#include<geometry_msgs/Pose.h>
#include<geometry_msgs/PoseStamped.h>

double ix, iy, px, py;

void poseCallback(const nav_msgs::Odometry &p_msg){
    px = p_msg.pose.pose.position.x;
    py = p_msg.pose.pose.position.y;
}

int main(int argc,char** argv){

    ros::init(argc, argv, "show_trajectory");
    ros::NodeHandle nh;
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("/trajectory", 1, true);
    ros::Subscriber pose_sub = nh.subscribe("/odom", 10, poseCallback);
    bool is_start = true;
    nav_msgs::Path path;
    while(ros::ok()){
        ros::spinOnce();
        if(is_start){
            ix = px;
            iy = py;
            is_start = false;
        }

        //打印运动轨迹
        
        path.header.stamp = ros::Time::now();
        path.header.frame_id = "/odom_combined";
        
        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = px-ix;
        this_pose_stamped.pose.position.y = py-iy;
        
        this_pose_stamped.pose.orientation.x = 0;
        this_pose_stamped.pose.orientation.y = 0;
        this_pose_stamped.pose.orientation.z = 0;
        this_pose_stamped.pose.orientation.w = 1;
        this_pose_stamped.header.stamp = ros::Time::now();
        this_pose_stamped.header.frame_id = "/odom_combined";

        path.poses.push_back(this_pose_stamped);
        path_pub.publish(path);

    }

    return 0;
}

控制机器人前进，并在Rviz中添加Path消息，Rviz中即可实时显示机器人的运动路径