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

最新推荐文章于 2024-07-15 14:34:51 发布

宁缺毋滥灬

最新推荐文章于 2024-07-15 14:34:51 发布

阅读量6.8k

点赞数 11

分类专栏： ROS 文章标签：算法 c++ linux ubuntu

本文链接：https://blog.csdn.net/weixin_42638797/article/details/118757025

版权

在Ubuntu 16.04和ROS Kinetic环境下，通过编写showpath_odom节点订阅odom消息，编译运行后，使用RVIZ添加Path显示轨迹。该方法适用于记录并展示小车的行驶路径，可扩展到其他轨迹获取场景。

摘要由CSDN通过智能技术生成

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(th