前言：
网上记录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订阅路径话题可见该路径已经生产成功
后记：
经过测试发现，这个路径启动一次只会发出一次，然后需要提前订阅上文章来源：https://www.toymoban.com/news/detail-694032.html
才会显示出来，Eojoy文章来源地址https://www.toymoban.com/news/detail-694032.html
到了这里，关于Ros noetic 机器人坐标记录运动路径和发布 实战教程（A）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
