ROS仿真机器人实现Rviz轨迹显示-Toy模板网

这篇具有很好参考价值的文章主要介绍了ROS仿真机器人实现Rviz轨迹显示。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

一、实现效果
红色为行驶过的轨迹
ros显示轨迹,ROS编程,ROS探索,机器人,c++ 二、实现方法
1、导航包中创建.cpp文件，并将以下代码复制进去

2、CMakeLists当中添加可执行文件及链接库3、启动导航的launch文件中添加启动该cpp文件

三、代码文章来源地址https://www.toymoban.com/news/detail-616175.html

#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 %.3lf\n", 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;
}