【ROS】将odom里程计数据转换为path路径消息

最新推荐文章于 2025-05-22 09:19:32 发布

JK-Cool

最新推荐文章于 2025-05-22 09:19:32 发布

阅读量5.1k

点赞数 6

CC 4.0 BY-SA版权

本文链接：https://blog.youkuaiyun.com/xingdou520/article/details/86480944

订阅底盘odom数据，转换为path数据；

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>

#include <nav_msgs/Odometry.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;
ros::Subscriber odomSub;
ros::Subscriber odom_raw_Sub;

 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.orientation = odom->pose.pose.orientation;

    this_pose_stamped.header.stamp = ros::Time::now();
    this_pose_stamped.header.frame