实时显示路径

lhy

在使用雷达建图完成之后，在该地图上进行导航，用手柄控制小车移动，怎么才能在地图上实时显示移动路径

WHEELTEC-Until

目前没有这种功能的，得编写程序实现，这种一般以里程计的数据进行实现，网上一般都有例子的

lhy

WHEELTEC-Until 发表于 2025-3-21 19:44
目前没有这种功能的，得编写程序实现，这种一般以里程计的数据进行实现，网上一般都有例子的

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry

class PathTracker:
    def __init__(self):
        rospy.init_node('path_tracker', anonymous=True)
        self.path_pub = rospy.Publisher('/robot_path', Path, queue_size=10)
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)

        self.path = Path()
        self.path.header.frame_id = "map"  # 设定路径的坐标系

    def odom_callback(self, msg):
        pose = PoseStamped()
        pose.header = msg.header
        pose.pose = msg.pose.pose
        self.path.poses.append(pose)

        # 限制存储的路径点数量，避免占用过多内存
        if len(self.path.poses) > 1000:
            self.path.poses.pop(0)

        self.path.header.stamp = rospy.Time.now()
        self.path_pub.publish(self.path)

if __name__ == '__main__':
    try:
        PathTracker()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

这是代码，显示路径为啥不在地图上，反而在z轴上，是代码有问题吗

admin

这个得自行检查一下代码是否有问题哈，我们不对自行编写的代码进行指导和检查的哈