MoveIt轨迹保存与回放报错求助（Ubuntu20.04+ROS Noetic）

问题背景 #

环境配置：Ubuntu 20.04、ROS Noetic、Python 3.8.10
需求：记录并回放MoveIt预规划轨迹，遇到以下报错：

1. YAML加载时报错：

could not determine a constructor for the tag 'tag:yaml.org,2002:python/tuple'
in "/home/saved_trajectories/test.yaml", line 1, column 1

2. JSON序列化时报错：

ERROR: b'error processing request: Object of type RobotTrajectory is not JSON serializable'

尝试yaml.safe_load(stream)未解决问题。

解决方案 #

方法一：ROS原生消息序列化（推荐，无类型兼容问题） #

利用ROS消息自带的序列化/反序列化功能，直接操作二进制文件，完全避免Python类型的序列化冲突。

保存轨迹

import rospy
from moveit_msgs.msg import RobotTrajectory

def save_trajectory(trajectory, file_path):
    # 将RobotTrajectory消息序列化为字节流
    serialized_data = trajectory.serialize()
    with open(file_path, 'wb') as f:
        f.write(serialized_data)

# 使用示例：替换为你的规划轨迹和保存路径
# save_trajectory(planned_trajectory, "/home/saved_trajectories/trajectory.bin")

加载并回放轨迹

import rospy
from moveit_msgs.msg import RobotTrajectory
from moveit_commander import MoveGroupCommander

def load_trajectory(file_path):
    trajectory = RobotTrajectory()
    with open(file_path, 'rb') as f:
        serialized_data = f.read()
    # 反序列化字节流为RobotTrajectory对象
    trajectory.deserialize(serialized_data)
    return trajectory

# 回放执行示例
if __name__ == "__main__":
    rospy.init_node("trajectory_player")
    # 替换为你的机械臂规划组名称
    move_group = MoveGroupCommander("your_arm_group")
    
    # 加载保存的轨迹
    trajectory = load_trajectory("/home/saved_trajectories/trajectory.bin")
    
    # 执行轨迹，wait=True表示等待执行完成
    move_group.execute(trajectory, wait=True)

方法二：通过字典转换实现YAML/JSON序列化 #

使用rospy_message_converter将ROS消息转换为Python字典，再用YAML/JSON工具保存，加载时转回消息对象。

安装依赖

sudo apt-get install ros-noetic-rospy-message-converter

保存轨迹到YAML

import rospy
from moveit_msgs.msg import RobotTrajectory
from rospy_message_converter import message_converter
import yaml

def save_trajectory_yaml(trajectory, file_path):
    # 将ROS消息转换为可序列化的字典
    traj_dict = message_converter.convert_ros_message_to_dictionary(trajectory)
    with open(file_path, 'w') as f:
        yaml.dump(traj_dict, f)

# 使用示例
# save_trajectory_yaml(planned_trajectory, "/home/saved_trajectories/trajectory.yaml")

加载YAML轨迹

import rospy
from moveit_msgs.msg import RobotTrajectory
from rospy_message_converter import message_converter
import yaml

def load_trajectory_yaml(file_path):
    with open(file_path, 'r') as f:
        traj_dict = yaml.safe_load(f)
    # 将字典转回RobotTrajectory消息对象
    trajectory = message_converter.convert_dictionary_to_ros_message('moveit_msgs/RobotTrajectory', traj_dict)
    return trajectory

# 回放代码同方法一的执行部分

报错原因说明 #

• 直接用yaml.dump保存ROS消息时，会写入Python专属的类型标记（如!!python/tuple），这些标记无法被yaml.safe_load解析，而yaml.load存在安全风险。
• JSON不支持序列化自定义ROS消息类对象，因为这些对象包含非原生JSON类型的属性。

内容的提问来源于stack exchange，提问作者ramyun