ROS 环境配置 PlanningScene

设置

可以使用RobotModel或URDF和SRDF轻松设置和配置PlanningScene类。 但是，这不是实例化PlanningScene的推荐方法。 PlanningSceneMonitor是使用机器人关节和机器人上的传感器创建和维护当前计划场景（并在下一个教程中详细讨论）的推荐方法。 在本教程中，我们将直接实例化一个PlanningScene类，但这种实例化

碰撞检测

自身碰撞检测

我们要做的第一件事是检查处于当前状态的机器人是否处于自碰撞状态，即机器人的当前配置是否会导致机器人的部件相互碰撞。 为此，我们将构造一个CollisionRequest对象和一个CollisionResult对象，并将它们传递给碰撞检查函数。 请注意，结果中包含机器人是否处于自碰撞的结果。 自碰撞检查使用机器人的无附加物版本，即它直接使用URDF中提供的碰撞网格而不添加额外的填充。

改变状态

现在，让我们改变机器人的当前状态。 PlanningScene在内部维护当前状态。 我们可以获得它的引用并进行更改，然后检查新机器人配置的冲突。 请特别注意，我们需要在发出新的碰撞检查请求之前清除collision_result。

检查一个组

现在，我们将仅对机器人的手进行碰撞检查，即我们将检查手与机器人身体的其他部分之间是否有任何碰撞。 我们可以通过在碰撞请求中添加组名“hand”来具体要求。

获取接触信息

首先，手动将熊猫臂设置到我们知道发生内部（自身）碰撞的位置。 请注意，此状态现在实际上超出了熊猫的关节限制，这是我们也可以直接检查出来的。

现在，我们可以获得可能在Panda臂的给定配置中发生的任何碰撞的接触信息。 我们可以通过填写碰撞请求中的相应字段并指定要作为大数返回的最大联系人数来请求联系信息。

修改允许碰撞矩阵（ACM）

AllowedCollisionMatrix（ACM）提供了一种机制，告诉我们可以忽略某些对象之间的碰撞：机器人的两个部分和世界上的对象。 我们可以告诉碰撞检查器忽略上面报告的链接之间的所有碰撞，即使链接实际上处于冲突中，碰撞检查器将忽略那些碰撞并且在机器人的这种特定状态下不返回碰撞。

在这个例子中还要注意我们如何复制允许的碰撞矩阵和当前状态，并将它们传递给碰撞检查功能。

完整的碰撞检查

当我们检查自冲突时，我们可以使用checkCollision函数，而不是检查自冲突和与环境的冲突(当前为空)。这是一组冲突检查函数，您将在计划器中最经常使用它们。注意，碰撞检查与环境将使用填充版本的机器人。填充物可以帮助机器人远离环境中的障碍物。

约束检测

PlanningScene类还包括用于检查约束的易于使用的函数调用。约束可以有两种类型:(a)从运动学约束集中选择的约束:关节约束、位置约束、方位约束和可视性约束;(b)通过回调指定的用户定义约束。我们首先来看一个具有简单运动学约束的例子。

检查运动学约束

我们将首先在熊猫机器人的panda_arm组的末端效应器上定义一个简单的位置和方向约束。 请注意使用便捷函数来填充约束（这些函数可以在moveit_core中的kinematic_constraints目录的utils.h文件中找到）。

现在，我们可以使用PlanningScene类中的isStateConstrained函数来检查针对此约束的状态。

有一种更有效的方法来检查约束（当你想要一遍又一遍地检查相同的约束时，例如在规划器内）。 我们首先构造一个KinematicConstraintSet，它预处理ROS约束消息并将其设置为快速处理。

使用KinematicConstraintSet类可以直接实现此目的。