ROS订阅odom里程计消息，并在RVIZ中显示轨迹

ubuntu16.04 kinetic

showpath

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
nav_msgs::Path  path;
ros::Publisher  path_pub;

void odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = odom -> pose.pose.position.x;
    this_pose_stamped.pose.position.y = odom -> pose.pose.position.y;
    this_pose_stamped.pose.position.z = odom -> pose.pose.position.z;

    this_pose_stamped.pose.orientation = odom -> pose.pose.orientation;

    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame_id = "map";

    path.poses.push_back(this_pose_stamped);

    path.header.stamp = ros::Time::now();
    path.header.frame_id = "map";
    path_pub.publish(path);
    printf("path_pub:");
    printf("odom %.3lf %.3lf %.3lfn", odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
}

int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath_odom");
    ros::NodeHandle ph;

    path_pub = ph.advertise<nav_msgs::Path>("trajectory_odom", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/odom", 10, odomCallback);  //订阅里程计话题信息
    
    ros::Rate loop_rate(50);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

编译之后

rosrun showpath_odom showpath_odom

运行结果如下

 然后打开RVIZ，点add ->path ,选择话题订阅trajectory_odom,运行小车，就能看到在rviz中的轨迹了，此算法包括但不仅限于记录odom发布的轨迹，同感修改订阅消息，也可完成动作或者其他问题的轨迹获取。

代码在这https://download.csdn.net/download/weixin_42638797/20299508

最后

以上就是沉静音响为你收集整理的ROS订阅odom里程计消息，并在RVIZ中显示轨迹的全部内容，希望文章能够帮你解决ROS订阅odom里程计消息，并在RVIZ中显示轨迹所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。