ros noetic moveit中的Warehouse - Persistent Scenes and States功能和使用方法

在ROS Noetic的MoveIt中，Warehouse是一个用于存储和检索MoveIt相关数据的工具。其中，Persistent Scenes and States功能允许用户将场景和机器人状态保存到Warehouse中，并在需要时检索它们。

使用Persistent Scenes and States功能需要进行以下步骤：

1. 创建一个Warehouse实例：

import moveit_warehouse
warehouse = moveit_warehouse.Warehouse('moveit_warehouse','robot_description')

2. 创建一个场景并将其保存到Warehouse中：

# 创建一个场景
scene = PlanningSceneInterface()
box_pose = PoseStamped()
box_pose.header.frame_id = "base_link"
box_pose.pose.position.x = 0.5
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = 0.1
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

# 将场景保存到Warehouse中
scene_msg = moveit_msgs.msg.PlanningSceneWorld()
scene.get_known_object_names(scene_msg.objects)
scene.get_known_object_colors(scene_msg.object_colors)
scene_stamped = moveit_msgs.msg.PlanningSceneStamped()
scene_stamped.scene = scene_msg
scene_stamped.robot_state.is_diff = True
scene_stamped.robot_state.joint_state.header.frame_id = "base_link"
scene_stamped.robot_state.joint_state.name = robot.get_joint_names()
scene_stamped.robot_state.joint_state.position = robot.get_current_joint_values()

# 设置场景的名字并保存到Warehouse中
scene_name = "my_scene"
scene_stamped_stored_id = warehouse.get_scene_store().store_scene(scene_stamped, scene_name)

3. 从Warehouse中检索场景：

# 从Warehouse中检索场景
scene_stamped_stored = warehouse.get_scene_store().get_scene_by_name(scene_name)
scene_stamped_stored.scene.world.collision_objects = scene_stamped_stored.scene.world.objects
scene_stamped_stored.robot_state.joint_state.header.frame_id = "base_link"
scene_stamped_stored.robot_state.is_diff = False
scene_stamped_stored.robot_state.joint_state.name = robot.get_joint_names()
scene_stamped_stored.robot_state.joint_state.position = robot.get_current_joint_values()

# 将场景加载到PlanningSceneInterface中
scene = PlanningSceneInterface()
scene.add_world_object_from_msg(scene_stamped_stored.scene.world, scene_stamped_stored.scene.robot_model_name)
robot.set_joint_value_target(scene_stamped_stored.robot_state.joint_state.position)

通过上述步骤，我们可以将场景和机器人状态保存到Warehouse中，并在需要时检索它们。这对于实现MoveIt中的复杂任务和调试非常有帮助。