ros noetic moveit中的Warehouse - Persistent Scenes and States功能python示例

以下是一个简单的 Python 示例，展示了如何使用 Warehouse - Persistent Scenes and States 功能：

import rospy
import moveit_commander
from moveit_commander import PlanningSceneInterface
from moveit_commander import RobotState
from moveit_commander import RobotTrajectory
from moveit_commander import MoveGroupCommander
from moveit_msgs.msg import PlanningScene, PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene, SaveMap, LoadMap
from trajectory_msgs.msg import JointTrajectoryPoint

def main():
    # 初始化 ROS 节点
    rospy.init_node('warehouse_tutorial', anonymous=True)

    # 初始化 MoveIt Commander
    robot = moveit_commander.RobotCommander()
    scene = PlanningSceneInterface()
    group = moveit_commander.MoveGroupCommander("manipulator")

    # 创建 Robot State 和 Joint Trajectory Point
    robot_state = RobotState()
    joint_trajectory_point = JointTrajectoryPoint()

    # 将 Robot State 中的关节状态设置为规划器的当前状态
    robot_state.joint_state.name = group.get_active_joints()
    robot_state.joint_state.position = group.get_current_joint_values()

    # 将 Joint Trajectory Point 中的关节状态设置为规划器的当前状态
    joint_trajectory_point.positions = group.get_current_joint_values()

    # 将 Robot State 和 Joint Trajectory Point 保存到 Warehouse 中
    rospy.wait_for_service("/planning_scene/get_planning_scene")
    get_planning_scene = rospy.ServiceProxy("/planning_scene/get_planning_scene", GetPlanningScene)
    response = get_planning_scene(PlanningSceneComponents(components=PlanningSceneComponents.ROBOT_STATE))
    robot_state.robot_model_state = response.scene.robot_state
    rospy.wait_for_service("/apply_planning_scene")
    apply_planning_scene = rospy.ServiceProxy("/apply_planning_scene", ApplyPlanningScene)
    apply_planning_scene(PlanningScene(robot_state=robot_state))

    rospy.wait_for_service("/planning_scene/get_planning_scene")
    get_planning_scene = rospy.ServiceProxy("/planning_scene/get_planning_scene", GetPlanningScene)
    response = get_planning_scene(PlanningSceneComponents(components=PlanningSceneComponents.ROBOT_STATE))
    robot_state.robot_model_state = response.scene.robot_state
    rospy.wait_for_service("/apply_planning_scene")
    apply_planning_scene = rospy.ServiceProxy("/apply_planning_scene", ApplyPlanningScene)
    apply_planning_scene(PlanningScene(robot_state=robot_state))

    rospy.wait_for_service("/planning_scene/get_planning_scene")
    get_planning_scene = rospy.ServiceProxy("/planning_scene/get_planning_scene", GetPlanningScene)
    response = get_planning_scene(PlanningSceneComponents(components=PlanningSceneComponents.JOINT_STATE))
    joint_trajectory_point.time_from_start = rospy.Duration(1.0)
    joint_trajectory_point.velocities = [0] * len(joint_trajectory_point.positions)
    robot_trajectory = RobotTrajectory()
    robot_trajectory.joint_trajectory.joint_names = response.scene.robot_state.joint_state.name
    robot_trajectory.joint_trajectory.points.append(joint_trajectory_point)
    rospy.wait_for_service("/save_map")
    save_map = rospy.ServiceProxy("/save_map", SaveMap)
    save_map(robot_trajectory, "trajectory")

    rospy.wait_for_service("/load_map")
    load_map = rospy.ServiceProxy("/load_map", LoadMap)
    robot_trajectory = load_map("trajectory")
    group.execute(robot_trajectory.joint_trajectory.points)

if __name__ == '__main__':
    main()

这个示例实现了以下功能：

1. 初始化 ROS 节点和 MoveIt Commander。
2. 创建 Robot State 和 Joint Trajectory Point。
3. 将 Robot State 和 Joint Trajectory Point 保存到 Warehouse 中。
4. 从 Warehouse 中加载 Joint Trajectory Point 并执行。

使用 Warehouse - Persistent Scenes and States 功能，您可以将 Robot State、Joint Trajectory Point 和 Robot Trajectory 保存到 Warehouse 中，并随时从 Warehouse 中加载和使用。这使得您可以在不同的 ROS Noetic 应用程序之间共享 Robot State、Joint Trajectory Point 和 Robot Trajectory，从而更方便地管理和重用这些数据。