- 在如何实现避障上，是ompl路径规划时，每生成一个新的State，就判断在该State是否有发生碰撞，这个判断过程叫碰撞检测，对应代码中函数StateValidityChecker::isValid。
- 碰撞检测是运动规划中最耗时运算，往往会占用90%左右的时间，但moveit好像没有提供参数可以关掉它。
- 因为解析时间过长，如果要加入避障，建议不要让urdf使用mesh类型的<collision>。
- moveit把环境中所有障碍物分到两棵层级树。一棵是机器人树，它包括机器人本体，即urdf上定义那些<link>。二是world树，它是通过调用psi.addCollisionObjects加入的障碍物。
机器人工作环境中会存在障碍物，避障就是要让机械臂运动过程中能避开它们。注意，机器人本体也是障碍物。举个例子，机器人有右手和左手，左手运动时要避开右手。
moveit把数个障碍物放在一个BVH。BVH是Bounding Volume Hierarchy的缩写，中文翻译成包围盒层级树、边界卷层级树。包围盒(BV)是几何的，包围某些模型的点(或三角形)的形状。一个简单例子是圆柱体，但还有其他更复杂的边框。对应到场景，可认为一个包围盒对应一个障碍物。BVH是包含着许多包围盒的树结构：根节点是包围整个模型的BV。然后该BV分为两个不同的子节点。子节点再次被拆分，递归地继续这个过程直到满足了终止条件。最后，树中的叶节点包含模型的实际点，即障碍物。BVH的优点是什么？ ——尽量减少碰撞检测的计算时间。设想下使用BVH时，机器人如何检测碰撞。要碰撞检测了，首先检查BVH的根BV是与和障碍物碰撞。如果没有碰撞，检测立即完成。如果发生碰撞，则须进入树结构，检查是否存在子结点BV与障碍物碰撞。如果其中一个发生碰撞，则进一步向下一级，递归检查其子结点。但是，如果其中一个没有碰撞，可以剔除挂在这个节点上的整个子树，节省了很多计算时间。只有当到达叶子节点时，才能看到机器人的实际障碍物。关于BVH可参考“Bounding Volume Hierarchies（BVH）的理解”。
moveit把环境中所有障碍物分到两棵层级树。一棵是机器人树，它包括机器人本体，即urdf上定义那些<link>。二是world树，它是通过调用psi.addCollisionObjects加入的障碍物，像机器人工作台。另外要注意，addCollisionObjects增加的障碍物，一旦后续以它为参数调用mg.attachObject，这个障碍物会离开world树，进入机器人树。
为实现避障，moveit采用FCL功能包（Flexible Collision Library，柔性避障库）。在如何实现避障上，是ompl路径规划时，每生成一个新的State，就判断在该State是否有发生碰撞，这个判断过程叫碰撞检测。如果检测通过，那这个State就是可用的，把它加入到有效路径点集合。否则扔掉，继续采下一个State。碰撞检测是运动规划中最耗时运算，往往会占用90%左右的时间，为了减少计算量，可以通过设置ACM(Allowed Collision Matrix)来进行优化。如果两个障碍物之间的ACM设置为1，则意味着这两个障碍物永远不会发生碰撞，不需要碰撞检测。Setup Assistant中“Self-coliisions”设置的自碰撞矩阵就是ACM的一部分。
一、client
moveit官方给了client示例：<moveit_ros>/planning_interface/test/move_group_interface_cpp_test.cpp，以下差不多就是去掉依赖<gtest/gtest.h>后的代码。
class MoveGroupTestFixture
{
public:
MoveGroupTestFixture(moveit::planning_interface::MoveGroupInterface& move_group, ros::CallbackQueue& cbqueue)
: move_group_(move_group)
, planning_scene_interface_(cbqueue)
{
SetUp();
}
void SetUp()
{
/* the tf buffer is not strictly needed,
but it's a simple way to add the codepaths to the tests */
psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description",
moveit::planning_interface::getSharedTF());
psm_->startSceneMonitor("/move_group/monitored_planning_scene");
const std::string DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
psm_->requestPlanningSceneState(DEFAULT_PLANNING_SCENE_SERVICE);
// give move_group_, planning_scene_interface_ and psm_ time to connect their topics
// ros::Duration(0.5).sleep();
}
// run updater() and ensure at least one geometry update was processed by the `move_group` node after doing sosynchronizeGeometryUpdate的功能是同步执行更新psm_内的几何结构。
@updater：要执行的更新操作。支持四种操作：planning_scene_interface_.addCollisionObjects、move_group_.attachObject、move_group_.detachObject、planning_scene_interface_.removeCollisionObjects。
void synchronizeGeometryUpdate(const std::function<void()>& updater)
{
SDL_Log("{MoveGroupTestFixture::synchronizeGeometryUpdate}---");
std::promise<void> promise;
std::future<void> future = promise.get_future();promise、future是std提供一种线程间同步机制，参考“[C++11]std::promise介绍及使用”。使用时分3步。1）创建一个promise、并由它得到一个future对象。2）线程A执行future.wait_for(seconds)，只要它的宿主promise没被set_value，线程A阻塞在在这个函数。3）线程B执行promise.set_value()。由于promise.set_value()，线程A结束future.wait_for(seconds)，执行下条指令。
psm_->addUpdateCallback([this, &promise](planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType t) {
if (t & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY) {
promise.set_value();只关注会设置PlanningSceneMonitor::UPDATE_GEOMETRY(更新几何结构)标记的操作。因为只有这四种操作，和该promise绑定的future正在wait_for。
} psm_->clearUpdateCallbacks(); }); updater(); // the updater must have triggered a geometry update, otherwise we can't be sure about the state of the scene anymore // std::future_status status = future.wait_for(std::chrono::seconds(5)); std::future_status status = future.wait_for(std::chrono::seconds(1200)); VALIDATE(status == std::future_status::ready, null_str); }
synchronizeGeometryUpdate触发更新操作updater()后，只是用ros机制发布了一个topic，发布完updater()语句就结束了。为确保psm_处理这话题，意味着需要继续等psm_接收到话题，并处理。而在处理完这话题后，psm_会调用这里addUpdateCallback设置的cb。于是client在这cb中唤醒future。换句话说，synchronizeGeometryUpdate会确保cb被调用后才返回，这实现了所谓的同步psm_更新操作。
void ModifyPlanningSceneAsyncInterfaces()
{
SDL_Log("{MoveGroupTestFixture::ModifyPlanningSceneAsyncInterfaces}---");
////////////////////////////////////////////////////////////////////
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 1.0;
primitive.dimensions[2] = 1.0;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.5;
box_pose.position.y = 0.0;
box_pose.position.z = 0.5;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
synchronizeGeometryUpdate([&]() { planning_scene_interface_.addCollisionObjects(collision_objects); });
size_t s = planning_scene_interface_.getObjects().size();
VALIDATE(s == collision_objects.size(), null_str);把障碍物box1增加到world树。
// attach and detach collision object
for (std::vector<moveit_msgs::CollisionObject>::const_iterator it = collision_objects.begin(); it != collision_objects.end(); ++ it) {
const moveit_msgs::CollisionObject& object = *it;
synchronizeGeometryUpdate([&]() {
bool ret = move_group_.attachObject(object.id, "base_link");
VALIDATE(ret, null_str);
});
}
s = planning_scene_interface_.getAttachedObjects().size();
VALIDATE(s == collision_objects.size(), null_str);attachObject把障碍物box1增加机器人树，同时从world树删除。
synchronizeGeometryUpdate([&]() {
bool ret = move_group_.detachObject(collision_object.id);
VALIDATE(ret, null_str);
});
s = planning_scene_interface_.getAttachedObjects().size();
VALIDATE(s == std::size_t(0), null_str);detachObject把障碍物box1从机器人树删除，同时增加到从world树。
// remove object from world
const std::vector<std::string> object_ids = { collision_object.id };
s = planning_scene_interface_.getObjects().size();
VALIDATE(s == std::size_t(1), null_str);
synchronizeGeometryUpdate([&]() { planning_scene_interface_.removeCollisionObjects(object_ids); });
s = planning_scene_interface_.getObjects().size();
VALIDATE(s == std::size_t(0), null_str);removeCollisionObjects从world树中移除障碍物box1。
}
private:
moveit::planning_interface::MoveGroupInterface& move_group_;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
planning_scene_monitor::PlanningSceneMonitorPtr psm_;
};
{
MoveGroupTestFixture test(group, cbqueue);
test.ModifyPlanningSceneAsyncInterfaces();在或mg.setPoseTargets，或mg.setPoseTarget，或computeCartesianPath，前先调用以下两个语句，作用是把障碍物box1增加到world树。
}
二、server
在server，引入避障逻辑是在路径规划算法生成一个新State时。
2.1 RRTConnect::growTree
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion)
{
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion);
.....
bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) :
si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
if (!validMotion)
return TRAPPED;dstate或是随机生成，或是插值生成，不论哪种情况，它是此次决定要加入扩展树的结点State。tgi.start=true，意味着此次是由tStart_树向tGoal_树，那移动方向是nmotion->start到dstate。否则tgi.start=false时，移动方向是dstate到nmotion->start。
...... return reach ? REACHED : ADVANCED; }
2.2 DiscreteMotionValidator::checkMotion
unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
{
return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
}
bool ompl::base::DiscreteMotionValidator::checkMotion(const State *s1, const State *s2) const
{
/* assume motion starts in a valid configuration so s1 is valid */
if (!si_->isValid(s2))
{
invalid_++;
return false;
}
bool result = true;
int nd = stateSpace_->validSegmentCount(s1, s2);上面有validSegmentCount()实现。longestValidSegment_是一个预置参数，示例0.0471。认为机械臂每次移动不会超过longestValidSegment_，于是一旦s1、s2距离超过longestValidSegment_，那要分段，返回值nd就是要s1、s2之间有分出的段数。
/* initialize the queue of test positions */
std::queue<std::pair<int, int>> pos;
if (nd >= 2)
{
pos.emplace(1, nd - 1);
/* temporary storage for the checked state */
State *test = si_->allocState();
/* repeatedly subdivide the path segment in the middle (and check the middle) */下面while作用是反复细分中间的路径段，并检查每个分隔点是否存在碰撞。
while (!pos.empty())
{
std::pair<int, int> x = pos.front();
int mid = (x.first + x.second) / 2;
stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, test);
if (!si_->isValid(test))
{
result = false;
break;
}
pos.pop();
if (x.first < mid)
pos.emplace(x.first, mid - 1);
if (x.second > mid)
pos.emplace(mid + 1, x.second);
}
si_->freeState(test);
}
if (result)
valid_++;
else
invalid_++;
return result;
}checkMotion逻辑说来就是把s1、s2之间距离分成nd段，并检测每个分隔点是否存在碰撞。在判断碰撞时，checkMotion调用的也是si_->isValid(state)，后者就是碰撞检测的主入口函数。
2.3 StateValidityChecker::isValid
- @state：要检查的结点状态。存储关节状态。
- @verbose：是否输出日志到控制台。不影响执行逻辑。
bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const
{
// Use cached validity if it is available
if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown())
return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
if (!si_->satisfiesBounds(state))
{
if (verbose)
ROS_INFO_NAMED(LOGNAME, "State outside bounds");
const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
return false;
}
moveit::core::RobotState* robot_state = tss_.getStateStorage();
planning_context_->getOMPLStateSpace()->copyToRobotState(*robot_state, state);经过copyToRobotState，state赋值给了robot_state中的关节状态。
// check path constraints
const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
if (kset && !kset->decide(*robot_state, verbose).satisfied)
{
const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
return false;
}
// check feasibility
if (!planning_context_->getPlanningScene()->isStateFeasible(*robot_state, verbose))
{
const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
return false;
}允许caller设置一个私有函数，返回值bool。功能是让增加个额外判断，该判断决定状态State被视为有效或无效，其原因超出了碰撞检查和约束评估所涵盖的原因。函数指针state_feasibility_指向这个私有函数，isStateFeasible就是调用这函数。对没有设置这函数的caller，返回true。
// check collision avoidance collision_detection::CollisionResult res; planning_context_->getPlanningScene()->checkCollision( verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *robot_state);
checkCollision执行着碰撞检测的核心逻辑。res.collision存储着判断结果，true表示在state发生了碰撞，否则没有。
if (!res.collision)
{
const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
}
else
{
const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
}
return !res.collision;
}2.4 PlanningScene::checkCollision
<moveit_core>/planning_scene/src/planning_scene.cpp>
------
void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const
{
// check collision with the world using the padded version
getCollisionEnv()->checkRobotCollision(req, res, robot_state, acm);getCollisionEnv()返回的类型是collision_detection::CollisionEnv。CollisionEnv封装了碰撞检测接口api。它是个抽像类，实用使用的是派生类，像用FCL算法实现的CollisionEnvFCL。
checkRobotCollision功能是检查机器人模型是否与world树发生碰撞。机器人<link>与world树之间的任何碰撞都会被考虑在内。但不检查自碰撞。
// do self-collision checking with the unpadded version of the robot if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts)) getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
getCollisionEnvUnpadded()返回的类型是也是CollisionEnv，实际使用的也是派生类CollisionEnvFCL，但它和上面的getCollisionEnv()不是同一个实例。
checkSelfCollision功能是检查机器人树中是否存在自碰撞，过程中会忽略预设置的自碰撞矩阵acm。
}
2.5 CollisionEnvFCL::checkRobotCollisionHelper
<moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp>
------
void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
{
checkRobotCollisionHelper(req, res, state, &acm);
}
void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix* acm) const
{
FCLObject fcl_obj;
constructFCLObjectRobot(state, fcl_obj);constructFCLObjectRobot把机器人树的障碍物填充到fcl_obj，包括机器人本身的link，以及attach到move_group的ob。
CollisionData cd(&req, &res, acm); cd.enableGroup(getRobotModel()); for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i) manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
此个manager_中存的是world树，挨个检查fcl_obj.collision_objects_中的obj，判断是否和world树存在碰撞。？一旦存在碰撞，cd.done_设置为true？
if (req.distance)
{
DistanceRequest dreq;
DistanceResult dres;
dreq.group_name = req.group_name;
dreq.acm = acm;
dreq.enableGroup(getRobotModel());
distanceRobot(dreq, dres, state);
res.distance = dres.minimum_distance.distance;
}
}2.6 CollisionEnvFCL::constructFCLObjectRobot
void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const
{
fcl_obj.collision_objects_.reserve(robot_geoms_.size());
fcl::Transform3d fcl_tf;
for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
{
transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
robot_geoms_[i]->collision_geometry_data_->shape_index),
fcl_tf);
auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
coll_obj->setTransform(fcl_tf);
coll_obj->computeAABB();
fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj));
}
// TODO: Implement a method for caching fcl::CollisionObject's for moveit::core::AttachedBody's
std::vector<const moveit::core::AttachedBody*> ab;
state.getAttachedBodies(ab);
for (auto& body : ab)
{
std::vector<FCLGeometryConstPtr> objs;
getAttachedBodyObjects(body, objs);
const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms();
for (std::size_t k = 0; k < objs.size(); ++k)
if (objs[k]->collision_geometry_)
{
transform2fcl(ab_t[k], fcl_tf);
fcl_obj.collision_objects_.push_back(
std::make_shared<fcl::CollisionObjectd>(objs[k]->collision_geometry_, fcl_tf));
// we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
// and would be destroyed when objs goes out of scope.
fcl_obj.collision_geometry_.push_back(objs[k]);
}
}
}
三、mesh类型的shape
shape类型是mesh时，速度很慢，这里给出三处耗时代码。为直观，同时给出具体消耗时间，运行的电脑是MacBookPro15,2。
- C/C++——Optimization——Optimization: Disabled(/Od)
- C/C++——Code Generation——Runtime Library: Multi-threaded(/MT)
综合来说，如果要加入避障，建议不要让urdf使用mesh类型的shape。
3.1 RobotModel::constructShape
JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model)
LinkModel* link = constructLinkModel(urdf_link);
shapes::ShapeConstPtr s = constructShape(col->geometry.get());
shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom)
{
moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
shapes::Shape* new_shape = nullptr;
switch (geom->type)
{
......
case urdf::Geometry::MESH:
{
const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
if (!mesh->filename.empty())
{
Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);由<mesh>标签构造出shapes::Mesh。[0/12]<mesh filename="package://tank_arm/.meshes/base_link.STL" />，耗时5.134秒。
[0/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/base_link.STL, )17 MB spend 5134 ms
[1/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/arm_Link.STL, )1,168 KB spend 1211 ms
[2/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/Link1.STL, )3,466 KB spend 3590 ms
[3/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/Link2.STL, )5,661 KB spend 6689 ms
[4/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/Link3.STL, )3,314 KB spend 1461 ms
[5/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/left_Link.STL, )107 KB spend 117 ms
[6/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/left_claw_Link.STL, )130 KB spend 136 ms
[7/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/right_Link.STL, )201 KB spend 194 ms
[8/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/right_claw_Link.STL, )130 KB spend 145 ms
[9/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/pan_Link.STL, )63 KB spend 65 ms
[10/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/pan_Link1.STL, )90 KB spend 74 ms
[11/12]...createMeshFromResource(mesh->filename: package://tank_arm/.meshes/pan_link2.STL, )773 KB spend 396 ms
new_shape = m; } } break; default: ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type); break; } return shapes::ShapePtr(new_shape); }
3.2 CollisionEnvFCL::CollisionEnvFCL
要调用createCollisionGeometry，从shapes::Shape创建出FCLGeometry，这也是个耗时操作。
CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
double scale)
: CollisionEnv(model, world, padding, scale)
{
const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
std::size_t index;
robot_geoms_.resize(robot_model_->getLinkGeometryCount());
robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
// we keep the same order of objects as what RobotState *::getLinkState() returns
for (auto link : links)
for (std::size_t j = 0; j < link->getShapes().size(); ++j)
{
FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
getLinkPadding(link->getName()), link, j);createCollisionGeometry是个耗时操作，links[0].shapes[0]耗时101.685秒，超过一分半。
[0/12][0]/[1]...createCollisionGeometry spend: 101685 ms
[1/12][0]/[1]...createCollisionGeometry spend: 5903 ms
[2/12][0]/[1]...createCollisionGeometry spend: 18728 ms
[3/12][0]/[1]...createCollisionGeometry spend: 31551 ms
[4/12][0]/[1]...createCollisionGeometry spend: 17682 ms
[5/12][0]/[1]...createCollisionGeometry spend: 435 ms
[6/12][0]/[1]...createCollisionGeometry spend: 544 ms
[7/12][0]/[1]...createCollisionGeometry spend: 866 ms
[8/12][0]/[1]...createCollisionGeometry spend: 546 ms
[9/12][0]/[1]...createCollisionGeometry spend: 232 ms
[10/12][0]/[1]...createCollisionGeometry spend: 353 ms
[11/12][0]/[1]...createCollisionGeometry spend: 3721 ms
... } manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>(); // request notifications about changes to new world observer_handle_ = getWorld()->addObserver( std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2)); getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE); }
3.3 PlanningSceneMonitor::excludeRobotLinksFromOctree
void PlanningSceneMonitor::excludeRobotLinksFromOctree()
{
......
for (const moveit::core::LinkModel* link : links)
{
std::vector<shapes::ShapeConstPtr> shapes = link->getShapes(); // copy shared ptrs on purpuse
for (std::size_t j = 0; j < shapes.size(); ++j)
{
// merge mesh vertices up to 0.1 mm apart
if (shapes[j]->type == shapes::MESH)
{
shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
m->mergeVertices(1e-4);mergeVertices是个非常耗时操作，links[0].shapes[0]耗时3631.258秒，超过了一小时。
[0/12][0/1]...m->mergeVertices(1e-4) spend 3631258 ms
[1/12][0/1]...m->mergeVertices(1e-4) spend 14088 ms
[2/12][0/1]...m->mergeVertices(1e-4) spend 102452 ms
[3/12][0/1]...m->mergeVertices(1e-4) spend 350707 ms
[4/12][0/1]...m->mergeVertices(1e-4) spend 116853 ms
[5/12][0/1]...m->mergeVertices(1e-4) spend 144 ms
[6/12][0/1]...m->mergeVertices(1e-4) spend 156 ms
[7/12][0/1]...m->mergeVertices(1e-4) spend 471 ms
[8/12][0/1]...m->mergeVertices(1e-4) spend 154 ms
[9/12][0/1]...m->mergeVertices(1e-4) spend 51 ms
[10/12][0/1]...m->mergeVertices(1e-4) spend 101 ms
[11/12][0/1]...m->mergeVertices(1e-4) spend 6893 ms
shapes[j].reset(m); } occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]); if (h) link_shape_handles_[link].push_back(std::make_pair(h, j)); } ...... } }