cartographer如何输出轨迹值

### 使用Cartographer输出轨迹值 为了获取由Cartographer SLAM库生成的轨迹数据，在ROS环境中可以通过订阅特定的话题来实现。具体来说，`/trajectory_nodes`话题提供了节点位置的信息，而`/landmark_poses`则可以用于接收地标的位置信息[^1]。 对于更详细的轨迹信息提取，通常会涉及到保存整个状态到`.pbstream`文件中，之后再处理这些文件以获得所需的数据。通过调用服务`/write_state`可将当前的地图和轨迹信息写入指定路径下的`.pbstream`文件内[^2]： ```bash rosservice call /write_state "{filename: '/path/to/output/file.pbstream'}" ``` 如果希望进一步分析轨迹的具体数值，可以在Python脚本或其他编程语言里解析此二进制格式的文件。官方提供了一个名为`cartographer.io.submap_pose_interpolator`工具类可以帮助读取并计算各时刻对应的位姿变换矩阵。 另外一种方法是在运行过程中直接监听发布频率较高的姿态估计消息，比如`/pose`或`/odometry/filtered`等主题下发布的PoseStamped类型的消息，它们包含了机器人每一刻的姿态估计结果，从而间接反映了轨迹的变化情况。 #### Python代码示例：监听并记录轨迹点 下面是一个简单的Python程序片段，用来订阅`/odom`话题并将接收到的里程计数据存储起来形成轨迹列表： ```python import rospy from nav_msgs.msg import Odometry import tf.transformations as trnsf class TrajectoryRecorder(object): def __init__(self, topic_name="/odom"): self._sub = rospy.Subscriber(topic_name, Odometry, self.callback) self.traj_points = [] def callback(self, msg): pose = msg.pose.pose.position quat = ( msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ) euler_angles = trnsf.euler_from_quaternion(quat) point_info = { "position": (pose.x, pose.y, pose.z), "orientation": euler_angles } self.traj_points.append(point_info) if __name__ == "__main__": rospy.init_node('record_trajectory') recorder = TrajectoryRecorder() try: rospy.spin() # Keep node alive until shutdown request is received. except KeyboardInterrupt: pass with open('/tmp/traj_data.txt', 'w') as f: for pt in recorder.traj_points: line = "%s %s\n" % str(pt["position"]), str(pt["orientation"]) f.write(line) ```