MoveIt!入门教程-ROS API规划场景

MoveIt!入门教程-ROS API规划场景

说明

• 本章使用两个操作来体验"planning scene diffs"的作用：
  • 在世界里增加或移除对象
  • 在机器人上增加和移除对象

ROS API

• ROS API（planning scene publisher） 通过主题接口diffs,进行规划场景发布。
• planning scene diff在当前的规划场景（由move_group维护）和新的用户期望的规划场景之间是不同的

发布必需的主题

• 注意：这个主题可能需要在launch文件重新映射

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
  ros::WallDuration sleep_t(0.5);
  sleep_t.sleep();
}

定义附加的对象信息

• 我们将使用这个消息来添加或去除来自世界的对象，并增加对象到机器人

moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "r_wrist_roll_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "r_wrist_roll_link";
/* The id of the object */
attached_object.object.id = "box";

/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;

/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;

attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);

• 注意：附加对象到机器人要求指定正确对应的操作作为ADD操作。

  attached_object.object.operation = attached_object.object.ADD;

增加对象到环境

• 增加对象到环境是通过增加到在规划场景的世界部分的冲突对象集合中来完成。
• 我们只使用attached_object消息的“object”区域

ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();

插曲：同步和异步更新

• 通过diffs与move_group节点进行交互有两种机制：

  • 通过rosservice调用发送一个diff，会阻塞直到diff被应用（同步更新）
  • 通过主题发送一个diff，会持续发送，不能确定diff是否被应用（异步更新）

• 本教程选择异步更新机制(针对可视化插入长时间的睡眠，异步更新机制就不构成问题)。但是通过service客户端替代planning_scene_diff_publisher也是很合理的。

ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();

• 通过service 调用发送diffs到规划场景:

moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);

• 注意：这里程序不会继续，直到我们确保diff被应用。

附加对象到机器人

• 当机器人从环境里捡起一个对象，我们就需要附加对象到机器人上。
• 因此，任何处理机器人模型的组件都知道要考虑到所附的对象，例如碰撞检查。
• 附加对象需要两个操作：
  • 从环境中移除原始对象
  • 附件对象到机器人

/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;

• 通过首先清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。

/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene);

sleep_time.sleep();

从机器人上减少对象

• 从机器人上减少对象需要两个操作：
  • 从机器人上减少对象
  • 重新引入对象到环境中

/* First, define the DETACH object message*/
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "r_wrist_roll_link";
detach_object.object.operation = attached_object.object.REMOVE;

• 通过首先清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。

/* Carry out the DETACH + ADD operation */
ROS_INFO("Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene_diff_publisher.publish(planning_scene);

sleep_time.sleep();

从冲突世界里删除对象

• 从冲突世界中删除对象只是需要使用较早定义的删除对象消息。值得注意的是如何清楚这些区域来确保diff消息不包含其他的附加对象或冲突对象。

ROS_INFO("Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher.publish(planning_scene);

完整代码

• 下载完整代码

编译代码

• 编译方法

launch文件

• 所需的launch文件

启动代码

roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch

理想输出

• 在Rviz，你可以看到：
  • 对象显示在规划场景里
  • 对象附加到机器人
  • 对象从机器人删除
  • 对象从规划场景删除