Ros noetic 机器人坐标记录运动路径和发布 实战教程（A）

前言：

         网上记录Path的写入文件看了一下还挺多的，有用yaml作为载体文件，也有用csv文件的路径信息，也有用txt来记录当前生成的路径信息，载体不重要，反正都是记录的方式，本文主要按yaml的方式写入，后文中将补全其余两种方式。

         其中两种方式的主要区别在于，加载yaml所需要的时间较长，而txt，csv读本时间短暂，而且csv文件的跨越性很广，轻小简单，cvs中可以写入bag，bag包中可以有很多信息包括坐标。

         最近在研究路径这一块，预期编写一套Ros节点

         1. 监听并记录机器人在键盘控制下运动的路径Path

         2. 将监听到的路径记录到本地Path.yaml文件中

         3.将记录的Path通过话题发布并在Rviz中显示出来

         4.成功结果如下

一、关于Path 消息的格式

       首先,我们需要在ROS中定义-一个路径消息类型,这个消息类型可以用来表示路径的起点终点和路径上的所有中间点。路径消息类型通常包含-个Header字段和一一个位于poses字段中的序列,该序列包含路径.上的所有中间点。在ROS中,路径消息类型通常是nav_ msgs: :Path。

       它包含了以下几个重要的信息:
●std_ msgs/Header header :该路径消息的头部信息,包括时间戳和坐标系信息。
●std::vector<geometry_ msgs/PoseStamped> poses t: 该路径消息包含的位姿信息的列表,是路径的关键部分。每个位姿信息是一个geometry_ msgs/PoseStamped类型,它包含以下几个重要的信息: .
        std_ msgs/Header header:该位姿信息的头部信息,与nav_ msgs::Path 中的header 相似。
        geometry_ msgs/Pose pose: 位姿信息,包括位置和方向。位置由一个geometry_ msgs/Point 类型表示,包括x、y和z三个坐标轴;向由一个geometry_ msgs/Quaternion 类型表示,包括四元数的四个分量。

通过nav_ msgs::Path类型消息中包含的这些信息,我们可以方便地表示出路径规划的结果,并用于实际的导航和控制中。
 

二、关于yaml的格式

其中yaml的格式如下

- header:
    frame_id: map
    seq: 1
    stamp: 1693461246680636882
  poses:
  - orientation:
      w: 0.0
      x: 0.0
      y: 0.0
      z: 0.0
    position:
      x: 0.31243622303009033
      y: 1.3675482273101807
      z: 0.0

说明：

每个位置数据包含以下字段：

header: 位置数据的头部信息，包括frame_id（坐标系名称）、seq（序列号）和stamp（时间戳）。
poses: 实际的位置信息列表，包含每个位置的姿态（orientation）和位置（position）。
以下是示例中给出的位置数据的解释：

头部信息：

frame_id: 坐标系名称为"map"。
seq: 序列号为1。
stamp: 时间戳为1693461246680636882。
第一个位置数据：

姿态信息：
w: 四元数的w分量为0.0。
x: 四元数的x分量为0.0。
y: 四元数的y分量为0.0。
z: 四元数的z分量为0.0。

位置信息：
x: X轴坐标为0.31243622303009033。
y: Y轴坐标为1.3675482273101807。
z: Z轴坐标为0.0。
每个位置数据包含一个唯一的头部信息和对应的姿态和位置信息。

三、代码主题思路，构架问题细节生成

        编写一个Ros节点用于record 键盘控制小车移动生成的Path

        之所以使用键盘控制操作小车在实际中行驶，是rviz中标定的点位不准确

        当然也可以使用Pub 四元函数手动发布当前值写入

        使用构架中我们选择通过自己操作

        这里有个点，我们都知道TF可以监听到map和小车base_link的坐标关系

        键盘节点控制小车在路上移动生成给人的感觉是一条路线

       但是TF树监听到的是一系列的坐标点的数据，所以这里应该会需要一个转换

       现在

      我们将用实际的行为来验证我这一猜想

四、编写Path的记录节点

#!/usr/bin/env python

import rospy
import tf2_ros
from geometry_msgs.msg import Twist, PoseStamped
import yaml

class LocationRecorder:
    def __init__(self):
        rospy.init_node('location_recorder', anonymous=True)

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
        self.location_path = []

    def location_callback(self, twist_msg):
        try:
            transform = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0), rospy.Duration(1.0))
            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position = transform.transform.translation
            pose.pose.orientation = transform.transform.rotation
            self.location_path.append(pose)
        except tf2_ros.LookupException as ex:
            rospy.logwarn(str(ex))
        except tf2_ros.ExtrapolationException as ex:
            rospy.logwarn(str(ex))

    def save_location_data(self):
        if self.location_path:
            location_data = {
                'header': {
                    'frame_id': 'map',
                    'seq': 1,
                    'stamp': str(rospy.Time.now().to_nsec())
                },
                'poses': []
            }
            for pose in self.location_path:
                pose_data = {
                    'orientation': {
                        'w': pose.pose.orientation.w,
                        'x': pose.pose.orientation.x,
                        'y': pose.pose.orientation.y,
                        'z': pose.pose.orientation.z
                    },
                    'position': {
                        'x': pose.pose.position.x,
                        'y': pose.pose.position.y,
                        'z': pose.pose.position.z
                    }
                }
                location_data['poses'].append(pose_data)

            with open('/path/to/Location.yaml', 'w') as file:
                yaml.dump(location_data, file)
            rospy.loginfo('Location data saved to Location.yaml')

    def spin(self):
        rate = rospy.Rate(10)  # 10Hz
        while not rospy.is_shutdown():
            self.save_location_data()
            rate.sleep()

if __name__ == '__main__':
    try:
        recorder = LocationRecorder()
        rospy.Subscriber('/cmd_vel', Twist, recorder.location_callback)
        recorder.spin()
    except rospy.ROSInterruptException:
        pass

代码流程解读

创建一个ROS节点，命名为"location_recorder"。

该节点的主要功能是记录机器人在地图上的位置信息并将其保存为YAML文件。

代码中的LocationRecorder类初始化了ROS节点、tf2_ros缓冲区、tf2_ros转换监听器和一个空的location_path列表。

在location_callback方法中，代码尝试通过tf2_ros缓冲区来查询"map"坐标系到"base_link"坐标系的变换信息。然后，它创建一个PoseStamped对象，将变换信息中的位置和方向赋值给这个对象，并将其添加到location_path列表中。

在save_location_data方法中，代码首先检查location_path列表是否为空。如果不为空，则创建一个字典location_data，用于存储位置信息。

location_data包含一个header字段和一个poses字段。

header字段包含框架ID、序列号和时间戳，

poses字段是一个列表，用于存储每个位置点的姿态信息。

然后，代码遍历location_path列表中的每个位置点，将姿态信息转换为字典格式，

并将其添加到location_data['poses']列表中。

最后，代码使用yaml.dump函数将location_data字典转换为YAML格式的字符串，并将其写入指定路径的文件中。

在spin方法中，代码设置了一个循环，以便以10Hz的频率连续保存位置数据。

它通过调用save_location_data方法来保存位置数据，并使用rospy.Rate对象来控制循环的频率。

在__main__部分，代码创建了一个LocationRecorder对象，并订阅了名为"/cmd_vel"的主题，用于获取机器人的速度信息。然后，它调用spin方法来启动节点的执行。

总的来说，该代码使用tf2_ros库来获取机器人在地图坐标系下的变换信息，并将其保存为YAML格式的文件。

它使用了ROS的发布-订阅模型来获取速度信息，并以一定的频率连续保存位置数据。


总体来说


初始化ROS节点和tf2_ros的缓冲区和监听器。

定义一个空列表location_path用于存储机器人位置数据。

实现location_callback回调函数，该函数会在接收到/cmd_vel话题发布的机器人速度消息时被调用。

在回调函数中，它会查询“world”坐标系到“robot_base”坐标系的变换，并将变换后的位置信息转换成PoseStamped消息，并将其添加到location_path列表中。

实现save_location_data函数，如果location_path列表非空，将位置数据保存到YAML文件Location.yaml中。

实现spin函数，在ROS节点运行期间以10Hz的频率调用save_location_data函数保存位置数据。

在__main__函数中，创建LocationRecorder对象，订阅/cmd_vel话题，并调用spin函数开始运行节点。

启动仿真节点

启动键盘节点并让小车运动，可见程序已经开始写入yaml

可见该节点已经使能并完整的输出到了yaml，我们现在打开yaml文件检查

由此可见我的所有数据都已经记录到了yaml的文件夹中

然后我们继续做吧，现在完成了记录，后一面中则需要将路径发布出来，然后我们继续做吧

五、装载并发布我所记录的yaml文件

由于我们之前安装Ros的格式完整记录并生成了我控制器中的所有数据

现在我们开始编写一个send_path的节点

用于装载我们之前写好的yaml文件

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Path
import yaml

def load_location_data():
    try:
        with open('/path/to/Location.yaml', 'r') as file:
            location_data = yaml.safe_load(file)
        return location_data
    except IOError:
        rospy.logerr('Failed to load Location.yaml')
        return None

def publish_path(location_data):
    if location_data and 'poses' in location_data:
        path_msg = Path()
        path_msg.header.frame_id = location_data['header']['frame_id']
        path_msg.header.stamp = rospy.Time.now()

        for pose_data in location_data['poses']:
            pose_msg = PoseStamped()
            pose_msg.header = path_msg.header
            pose_msg.pose.position.x = pose_data['position']['x']
            pose_msg.pose.position.y = pose_data['position']['y']
            pose_msg.pose.position.z = pose_data['position']['z']
            pose_msg.pose.orientation.w = pose_data['orientation']['w']
            pose_msg.pose.orientation.x = pose_data['orientation']['x']
            pose_msg.pose.orientation.y = pose_data['orientation']['y']
            pose_msg.pose.orientation.z = pose_data['orientation']['z']

            path_msg.poses.append(pose_msg)

        path_publisher.publish(path_msg)
        rospy.loginfo('Path published to /send_path topic')

if __name__ == '__main__':
    try:
        rospy.init_node('path_publisher', anonymous=True)
        path_publisher = rospy.Publisher('/send_path', Path, queue_size=10)
        
        location_data = load_location_data()
        publish_path(location_data)

        rospy.spin()

    except rospy.ROSInterruptException:
        pass

代码解读：

在这个例子中，我们导入了rospy用于ROS功能的Python接口，以及nav_msgs.msg用于Path消息类型的导入。

接下来，我们定义了一个名为load_location_data的函数。

这个函数用于加载Location.yaml文件并返回其中的位置数据。

我们使用yaml.safe_load来加载文件，并将结果存储在location_data中。

如果文件读取失败，我们将打印出错误信息并返回None。

然后，我们定义了一个名为publish_path的函数。

这个函数接收位置数据作为输入，并将其转换为Path消息类型并发布到/send_path话题上。

我们首先创建一个Path消息对象，并设置其frame_id和时间戳。接着，我们遍历位置数据中的每个姿态数据，创建PoseStamped消息对象，并将其添加到Path消息中。

最后，我们通过path_publisher发布Path消息，并打印出相应的日志信息。

在main函数中，我们初始化了ROS节点并创建了一个名为path_publisher的发布者对象，用于发布Path消息。

然后，我们加载位置数据并通过publish_path函数将其发布。

最后，我们通过调用rospy.spin()来运行节点。

请确保在运行该节点之前，将脚本中的/path/to/Location.yaml替换为实际的文件路径。

此节点将加载Location.yaml文件中的位置数据，并将其作为Path消息发布到/send_path话题上。

启动路径的发布节点

我们使用rostopic list echo 查看 我们发布的路径话题中是否存在/send_path

最后点击开Rviz订阅路径话题可见该路径已经生产成功

后记：

经过测试发现，这个路径启动一次只会发出一次，然后需要提前订阅上

才会显示出来，Eojoy