00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <boost/algorithm/string.hpp>
00038 #include <moveit/planning_scene/planning_scene.h>
00039 #include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
00040 #include <geometric_shapes/shape_operations.h>
00041 #include <moveit/collision_detection/collision_tools.h>
00042 #include <moveit/trajectory_processing/trajectory_tools.h>
00043 #include <moveit/robot_state/conversions.h>
00044 #include <moveit/exceptions/exceptions.h>
00045 #include <octomap_msgs/conversions.h>
00046 #include <eigen_conversions/eigen_msg.h>
00047 #include <set>
00048
00049 namespace planning_scene
00050 {
00051 const std::string PlanningScene::OCTOMAP_NS = "<octomap>";
00052 const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)";
00053
00054 class SceneTransforms : public robot_state::Transforms
00055 {
00056 public:
00057 SceneTransforms(const PlanningScene *scene) :
00058 Transforms(scene->getRobotModel()->getModelFrame()),
00059 scene_(scene)
00060 {
00061 }
00062
00063 virtual bool canTransform(const std::string &from_frame) const
00064 {
00065 return scene_->knowsFrameTransform(from_frame);
00066 }
00067
00068 virtual bool isFixedFrame(const std::string &frame) const
00069 {
00070 if (frame.empty())
00071 return false;
00072 if (Transforms::isFixedFrame(frame))
00073 return true;
00074 if (frame[0] == '/')
00075 return knowsObject(frame.substr(1));
00076 else
00077 return knowsObject(frame);
00078 }
00079
00080 virtual const Eigen::Affine3d& getTransform(const std::string &from_frame) const
00081 {
00082 return scene_->getFrameTransform(from_frame);
00083 }
00084
00085 private:
00086
00087 bool knowsObject(const std::string &id) const
00088 {
00089 if (scene_->getWorld()->hasObject(id))
00090 {
00091 collision_detection::World::ObjectConstPtr obj = scene_->getWorld()->getObject(id);
00092 return obj->shape_poses_.size() == 1;
00093 }
00094 return false;
00095 }
00096
00097 const PlanningScene *scene_;
00098 };
00099
00100 }
00101
00102 bool planning_scene::PlanningScene::isEmpty(const moveit_msgs::PlanningScene &msg)
00103 {
00104 return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() &&
00105 msg.link_padding.empty() && msg.link_scale.empty() && isEmpty(msg.robot_state) && isEmpty(msg.world);
00106 }
00107
00108 bool planning_scene::PlanningScene::isEmpty(const moveit_msgs::RobotState &msg)
00109 {
00110
00111
00112 return msg.is_diff == true && msg.multi_dof_joint_state.joint_names.empty() && msg.joint_state.name.empty() &&
00113 msg.attached_collision_objects.empty() && msg.joint_state.position.empty() && msg.joint_state.velocity.empty() &&
00114 msg.joint_state.effort.empty() && msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() &&
00115 msg.multi_dof_joint_state.wrench.empty();
00116 }
00117
00118 bool planning_scene::PlanningScene::isEmpty(const moveit_msgs::PlanningSceneWorld &msg)
00119 {
00120 return msg.collision_objects.empty() && msg.octomap.octomap.data.empty();
00121 }
00122
00123 planning_scene::PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr &robot_model,
00124 collision_detection::WorldPtr world) :
00125 kmodel_(robot_model),
00126 world_(world),
00127 world_const_(world)
00128 {
00129 initialize();
00130 }
00131
00132 planning_scene::PlanningScene::PlanningScene(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00133 const boost::shared_ptr<const srdf::Model> &srdf_model,
00134 collision_detection::WorldPtr world) :
00135 world_(world),
00136 world_const_(world)
00137 {
00138 if (!urdf_model)
00139 throw moveit::ConstructException("The URDF model cannot be NULL");
00140
00141 if (!srdf_model)
00142 throw moveit::ConstructException("The SRDF model cannot be NULL");
00143
00144 kmodel_ = createRobotModel(urdf_model, srdf_model);
00145 if (!kmodel_)
00146 throw moveit::ConstructException("Could not create RobotModel");
00147
00148 initialize();
00149 }
00150
00151 planning_scene::PlanningScene::~PlanningScene()
00152 {
00153 if (current_world_object_update_callback_)
00154 world_->removeObserver(current_world_object_update_observer_handle_);
00155 }
00156
00157 void planning_scene::PlanningScene::initialize()
00158 {
00159 name_ = DEFAULT_SCENE_NAME;
00160
00161 ftf_.reset(new SceneTransforms(this));
00162
00163 kstate_.reset(new robot_state::RobotState(kmodel_));
00164 kstate_->setToDefaultValues();
00165
00166 acm_.reset(new collision_detection::AllowedCollisionMatrix());
00167
00168 const std::vector<std::string>& collision_links = kmodel_->getLinkModelNamesWithCollisionGeometry();
00169 acm_->setEntry(collision_links, collision_links, false);
00170
00171
00172 const std::vector<srdf::Model::DisabledCollision> &dc = getRobotModel()->getSRDF()->getDisabledCollisionPairs();
00173 for (std::vector<srdf::Model::DisabledCollision>::const_iterator it = dc.begin(); it != dc.end(); ++it)
00174 acm_->setEntry(it->link1_, it->link2_, true);
00175
00176 setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
00177 }
00178
00179
00180 robot_model::RobotModelPtr planning_scene::PlanningScene::createRobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00181 const boost::shared_ptr<const srdf::Model> &srdf_model)
00182 {
00183 robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf_model, srdf_model));
00184 if (!robot_model || !robot_model->getRootJoint())
00185 return robot_model::RobotModelPtr();
00186
00187 return robot_model;
00188 }
00189
00190 planning_scene::PlanningScene::PlanningScene(const PlanningSceneConstPtr &parent) :
00191 parent_(parent)
00192 {
00193 if (!parent_)
00194 throw moveit::ConstructException("NULL parent pointer for planning scene");
00195
00196 if (!parent_->getName().empty())
00197 name_ = parent_->getName() + "+";
00198
00199 kmodel_ = parent_->kmodel_;
00200
00201
00202
00203 world_.reset(new collision_detection::World(*parent_->world_));
00204 world_const_ = world_;
00205
00206
00207 world_diff_.reset(new collision_detection::WorldDiff(world_));
00208
00209
00210 for (CollisionDetectorConstIterator it = parent_->collision_.begin() ; it != parent_->collision_.end() ; ++it)
00211 {
00212 const CollisionDetectorPtr& parent_detector = it->second;
00213 CollisionDetectorPtr& detector = collision_[it->first];
00214 detector.reset(new CollisionDetector());
00215 detector->alloc_ = parent_detector->alloc_;
00216 detector->parent_ = parent_detector;
00217
00218 detector->cworld_ = detector->alloc_->allocateWorld(parent_detector->cworld_, world_);
00219 detector->cworld_const_ = detector->cworld_;
00220
00221
00222
00223 detector->crobot_.reset();
00224 detector->crobot_const_.reset();
00225 detector->crobot_unpadded_.reset();
00226 detector->crobot_unpadded_const_.reset();
00227 }
00228 setActiveCollisionDetector(parent_->getActiveCollisionDetectorName());
00229 }
00230
00231 planning_scene::PlanningScenePtr planning_scene::PlanningScene::clone(const planning_scene::PlanningSceneConstPtr &scene)
00232 {
00233 PlanningScenePtr result = scene->diff();
00234 result->decoupleParent();
00235 result->setName(scene->getName());
00236 return result;
00237 }
00238
00239 planning_scene::PlanningScenePtr planning_scene::PlanningScene::diff() const
00240 {
00241 return PlanningScenePtr(new PlanningScene(shared_from_this()));
00242 }
00243
00244 planning_scene::PlanningScenePtr planning_scene::PlanningScene::diff(const moveit_msgs::PlanningScene &msg) const
00245 {
00246 planning_scene::PlanningScenePtr result = diff();
00247 result->setPlanningSceneDiffMsg(msg);
00248 return result;
00249 }
00250
00251 void planning_scene::PlanningScene::CollisionDetector::copyPadding(const planning_scene::PlanningScene::CollisionDetector& src)
00252 {
00253 if (!crobot_)
00254 {
00255 alloc_->allocateRobot(parent_->getCollisionRobot());
00256 crobot_const_ = crobot_;
00257 }
00258
00259 crobot_->setLinkPadding(src.getCollisionRobot()->getLinkPadding());
00260 crobot_->setLinkScale(src.getCollisionRobot()->getLinkScale());
00261 }
00262
00263 void planning_scene::PlanningScene::propogateRobotPadding()
00264 {
00265 if (!active_collision_->crobot_)
00266 return;
00267
00268 for (CollisionDetectorIterator it = collision_.begin() ; it != collision_.end() ; ++it)
00269 {
00270 if (it->second != active_collision_)
00271 it->second->copyPadding(*active_collision_);
00272 }
00273 }
00274
00275 void planning_scene::PlanningScene::CollisionDetector::findParent(const PlanningScene& scene)
00276 {
00277 if (parent_ || !scene.parent_)
00278 return;
00279
00280 CollisionDetectorConstIterator it = scene.parent_->collision_.find(alloc_->getName());
00281 if (it != scene.parent_->collision_.end())
00282 parent_ = it->second->parent_;
00283 }
00284
00285 void planning_scene::PlanningScene::addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
00286 {
00287 const std::string& name = allocator->getName();
00288 CollisionDetectorPtr& detector = collision_[name];
00289
00290 if (detector)
00291 return;
00292
00293 detector.reset(new CollisionDetector());
00294
00295 detector->alloc_ = allocator;
00296
00297 if (!active_collision_)
00298 active_collision_ = detector;
00299
00300 detector->findParent(*this);
00301
00302 detector->cworld_ = detector->alloc_->allocateWorld(world_);
00303 detector->cworld_const_ = detector->cworld_;
00304
00305
00306
00307 if (!detector->parent_ || active_collision_->crobot_)
00308 {
00309 detector->crobot_ = detector->alloc_->allocateRobot(getRobotModel());
00310 detector->crobot_const_ = detector->crobot_;
00311
00312 if (detector != active_collision_)
00313 detector->copyPadding(*active_collision_);
00314 }
00315
00316
00317 if (!detector->parent_)
00318 {
00319 detector->crobot_unpadded_ = detector->alloc_->allocateRobot(getRobotModel());
00320 detector->crobot_unpadded_const_ = detector->crobot_unpadded_;
00321 }
00322 }
00323
00324 void planning_scene::PlanningScene::setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
00325 bool exclusive)
00326 {
00327 if (exclusive)
00328 {
00329 CollisionDetectorPtr p;
00330 CollisionDetectorIterator it = collision_.find(allocator->getName());
00331 if (it != collision_.end())
00332 p = it->second;
00333
00334 collision_.clear();
00335 active_collision_.reset();
00336
00337 if (p)
00338 {
00339 collision_[allocator->getName()] = p;
00340 active_collision_ = p;
00341 return;
00342 }
00343 }
00344
00345 addCollisionDetector(allocator);
00346 setActiveCollisionDetector(allocator->getName());
00347 }
00348
00349
00350 bool planning_scene::PlanningScene::setActiveCollisionDetector(const std::string& collision_detector_name)
00351 {
00352 CollisionDetectorIterator it = collision_.find(collision_detector_name);
00353 if (it != collision_.end())
00354 {
00355 active_collision_ = it->second;
00356 return true;
00357 }
00358 else
00359 {
00360 logError("Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. Keeping existing active collision detector '%s'",
00361 collision_detector_name.c_str(),
00362 active_collision_->alloc_->getName().c_str());
00363 return false;
00364 }
00365 }
00366
00367 void planning_scene::PlanningScene::getCollisionDetectorNames(std::vector<std::string>& names) const
00368 {
00369 names.clear();
00370 names.reserve(collision_.size());
00371 for (CollisionDetectorConstIterator it = collision_.begin() ; it != collision_.end() ; ++it)
00372 names.push_back(it->first);
00373 }
00374
00375 const collision_detection::CollisionWorldConstPtr& planning_scene::PlanningScene::getCollisionWorld(
00376 const std::string& collision_detector_name) const
00377 {
00378 CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
00379 if (it == collision_.end())
00380 {
00381 logError("Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead",
00382 collision_detector_name.c_str(),
00383 active_collision_->alloc_->getName().c_str());
00384 return active_collision_->cworld_const_;
00385 }
00386
00387 return it->second->cworld_const_;
00388 }
00389
00390 const collision_detection::CollisionRobotConstPtr& planning_scene::PlanningScene::getCollisionRobot(
00391 const std::string& collision_detector_name) const
00392 {
00393 CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
00394 if (it == collision_.end())
00395 {
00396 logError("Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
00397 collision_detector_name.c_str(),
00398 active_collision_->alloc_->getName().c_str());
00399 return active_collision_->getCollisionRobot();
00400 }
00401
00402 return it->second->getCollisionRobot();
00403 }
00404
00405 const collision_detection::CollisionRobotConstPtr& planning_scene::PlanningScene::getCollisionRobotUnpadded(
00406 const std::string& collision_detector_name) const
00407 {
00408 CollisionDetectorConstIterator it = collision_.find(collision_detector_name);
00409 if (it == collision_.end())
00410 {
00411 logError("Could not get CollisionRobotUnpadded named '%s'. Returning active CollisionRobotUnpadded '%s' instead",
00412 collision_detector_name.c_str(),
00413 active_collision_->alloc_->getName().c_str());
00414 return active_collision_->getCollisionRobotUnpadded();
00415 }
00416
00417 return it->second->getCollisionRobotUnpadded();
00418 }
00419
00420
00421 void planning_scene::PlanningScene::clearDiffs()
00422 {
00423 if (!parent_)
00424 return;
00425
00426
00427 world_.reset(new collision_detection::World(*parent_->world_));
00428 world_const_ = world_;
00429 world_diff_.reset(new collision_detection::WorldDiff(world_));
00430 if (current_world_object_update_callback_)
00431 current_world_object_update_observer_handle_ = world_->addObserver(current_world_object_update_callback_);
00432
00433
00434 for (CollisionDetectorIterator it = collision_.begin() ; it != collision_.end() ; ++it)
00435 {
00436 if (!it->second->parent_)
00437 it->second->findParent(*this);
00438
00439 if (it->second->parent_)
00440 {
00441 it->second->crobot_.reset();
00442 it->second->crobot_const_.reset();
00443 it->second->crobot_unpadded_.reset();
00444 it->second->crobot_unpadded_const_.reset();
00445
00446 it->second->cworld_ = it->second->alloc_->allocateWorld(it->second->parent_->cworld_, world_);
00447 it->second->cworld_const_ = it->second->cworld_;
00448 }
00449 else
00450 {
00451 it->second->copyPadding(*parent_->active_collision_);
00452
00453 it->second->cworld_ = it->second->alloc_->allocateWorld(world_);
00454 it->second->cworld_const_ = it->second->cworld_;
00455 }
00456 }
00457
00458 ftf_.reset();
00459 kstate_.reset();
00460 acm_.reset();
00461 object_colors_.reset();
00462 object_types_.reset();
00463 }
00464
00465 void planning_scene::PlanningScene::pushDiffs(const PlanningScenePtr &scene)
00466 {
00467 if (!parent_)
00468 return;
00469
00470 if (ftf_)
00471 scene->getTransformsNonConst().setAllTransforms(ftf_->getAllTransforms());
00472
00473 if (kstate_)
00474 scene->getCurrentStateNonConst() = *kstate_;
00475
00476 if (acm_)
00477 scene->getAllowedCollisionMatrixNonConst() = *acm_;
00478
00479 if (active_collision_->crobot_)
00480 {
00481 collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst();
00482 active_crobot->setLinkPadding(active_collision_->crobot_->getLinkPadding());
00483 active_crobot->setLinkScale(active_collision_->crobot_->getLinkScale());
00484 scene->propogateRobotPadding();
00485 }
00486
00487 if (world_diff_)
00488 {
00489 for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin() ; it != world_diff_->end() ; ++it)
00490 {
00491 if (it->second == collision_detection::World::DESTROY)
00492 {
00493 scene->world_->removeObject(it->first);
00494 scene->removeObjectColor(it->first);
00495 }
00496 else
00497 {
00498 const collision_detection::World::Object& obj = *world_->getObject(it->first);
00499 scene->world_->removeObject(obj.id_);
00500 scene->world_->addToObject(obj.id_, obj.shapes_, obj.shape_poses_);
00501 if (hasObjectColor(it->first))
00502 scene->setObjectColor(it->first, getObjectColor(it->first));
00503 if (hasObjectType(it->first))
00504 scene->setObjectType(it->first, getObjectType(it->first));
00505 }
00506 }
00507 }
00508 }
00509
00510 void planning_scene::PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult &res)
00511 {
00512 if (getCurrentState().dirtyCollisionBodyTransforms())
00513 checkCollision(req, res, getCurrentStateNonConst());
00514 else
00515 checkCollision(req, res, getCurrentState());
00516 }
00517
00518 void planning_scene::PlanningScene::checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult &res,
00519 const robot_state::RobotState &kstate) const
00520 {
00521
00522 getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, getAllowedCollisionMatrix());
00523
00524 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00525 {
00526
00527 getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, getAllowedCollisionMatrix());
00528 }
00529 }
00530
00531 void planning_scene::PlanningScene::checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult &res)
00532 {
00533 if (getCurrentState().dirtyCollisionBodyTransforms())
00534 checkSelfCollision(req, res, getCurrentStateNonConst());
00535 else
00536 checkSelfCollision(req, res, getCurrentState());
00537 }
00538
00539 void planning_scene::PlanningScene::checkCollision(const collision_detection::CollisionRequest& req,
00540 collision_detection::CollisionResult &res,
00541 const robot_state::RobotState &kstate,
00542 const collision_detection::AllowedCollisionMatrix& acm) const
00543 {
00544
00545 getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, acm);
00546
00547
00548 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00549 getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
00550 }
00551
00552 void planning_scene::PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00553 collision_detection::CollisionResult &res)
00554 {
00555 if (getCurrentState().dirtyCollisionBodyTransforms())
00556 checkCollisionUnpadded(req, res, getCurrentStateNonConst(), getAllowedCollisionMatrix());
00557 else
00558 checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix());
00559 }
00560
00561
00562 void planning_scene::PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
00563 collision_detection::CollisionResult &res,
00564 const robot_state::RobotState &kstate,
00565 const collision_detection::AllowedCollisionMatrix& acm) const
00566 {
00567
00568 getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), kstate, acm);
00569
00570
00571 if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00572 {
00573 getCollisionRobotUnpadded()->checkSelfCollision(req, res, kstate, acm);
00574 }
00575 }
00576
00577 void planning_scene::PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts)
00578 {
00579 if (getCurrentState().dirtyCollisionBodyTransforms())
00580 getCollidingPairs(contacts, getCurrentStateNonConst(), getAllowedCollisionMatrix());
00581 else
00582 getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
00583 }
00584
00585 void planning_scene::PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts,
00586 const robot_state::RobotState &kstate,
00587 const collision_detection::AllowedCollisionMatrix& acm) const
00588 {
00589 collision_detection::CollisionRequest req;
00590 req.contacts = true;
00591 req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
00592 req.max_contacts_per_pair = 1;
00593 collision_detection::CollisionResult res;
00594 checkCollision(req, res, kstate, acm);
00595 res.contacts.swap(contacts);
00596 }
00597
00598 void planning_scene::PlanningScene::getCollidingLinks(std::vector<std::string> &links)
00599 {
00600 if (getCurrentState().dirtyCollisionBodyTransforms())
00601 getCollidingLinks(links, getCurrentStateNonConst(), getAllowedCollisionMatrix());
00602 else
00603 getCollidingLinks(links, getCurrentState(), getAllowedCollisionMatrix());
00604 }
00605
00606 void planning_scene::PlanningScene::getCollidingLinks(std::vector<std::string> &links,
00607 const robot_state::RobotState &kstate,
00608 const collision_detection::AllowedCollisionMatrix& acm) const
00609 {
00610 collision_detection::CollisionResult::ContactMap contacts;
00611 getCollidingPairs(contacts, kstate, acm);
00612 links.clear();
00613 for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin() ; it != contacts.end() ; ++it)
00614 for (std::size_t j = 0 ; j < it->second.size() ; ++j)
00615 {
00616 if (it->second[j].body_type_1 == collision_detection::BodyTypes::ROBOT_LINK)
00617 links.push_back(it->second[j].body_name_1);
00618 if (it->second[j].body_type_2 == collision_detection::BodyTypes::ROBOT_LINK)
00619 links.push_back(it->second[j].body_name_2);
00620 }
00621 }
00622
00623 const collision_detection::CollisionRobotPtr& planning_scene::PlanningScene::getCollisionRobotNonConst()
00624 {
00625 if (!active_collision_->crobot_)
00626 {
00627 active_collision_->crobot_ = active_collision_->alloc_->allocateRobot(active_collision_->parent_->getCollisionRobot());
00628 active_collision_->crobot_const_ = active_collision_->crobot_;
00629 }
00630 return active_collision_->crobot_;
00631 }
00632
00633 robot_state::RobotState& planning_scene::PlanningScene::getCurrentStateNonConst()
00634 {
00635 if (!kstate_)
00636 {
00637 kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
00638 kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
00639 }
00640 kstate_->update();
00641 return *kstate_;
00642 }
00643
00644 robot_state::RobotStatePtr planning_scene::PlanningScene::getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
00645 {
00646 robot_state::RobotStatePtr state(new robot_state::RobotState(getCurrentState()));
00647 robot_state::robotStateMsgToRobotState(getTransforms(), update, *state);
00648 return state;
00649 }
00650
00651 void planning_scene::PlanningScene::setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback)
00652 {
00653 current_state_attached_body_callback_ = callback;
00654 if (kstate_)
00655 kstate_->setAttachedBodyUpdateCallback(callback);
00656 }
00657
00658 void planning_scene::PlanningScene::setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
00659 {
00660 if (current_world_object_update_callback_)
00661 world_->removeObserver(current_world_object_update_observer_handle_);
00662 if (callback)
00663 current_world_object_update_observer_handle_ = world_->addObserver(callback);
00664 current_world_object_update_callback_ = callback;
00665 }
00666
00667 collision_detection::AllowedCollisionMatrix& planning_scene::PlanningScene::getAllowedCollisionMatrixNonConst()
00668 {
00669 if (!acm_)
00670 acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
00671 return *acm_;
00672 }
00673
00674 const robot_state::Transforms& planning_scene::PlanningScene::getTransforms()
00675 {
00676 getCurrentStateNonConst().update();
00677 return const_cast<const PlanningScene*>(this)->getTransforms();
00678 }
00679
00680 robot_state::Transforms& planning_scene::PlanningScene::getTransformsNonConst()
00681 {
00682 getCurrentStateNonConst().update();
00683 if (!ftf_)
00684 {
00685 ftf_.reset(new SceneTransforms(this));
00686 ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
00687 }
00688 return *ftf_;
00689 }
00690
00691 void planning_scene::PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene_msg) const
00692 {
00693 scene_msg.name = name_;
00694 scene_msg.robot_model_name = getRobotModel()->getName();
00695 scene_msg.is_diff = true;
00696
00697 if (ftf_)
00698 ftf_->copyTransforms(scene_msg.fixed_frame_transforms);
00699 else
00700 scene_msg.fixed_frame_transforms.clear();
00701
00702 if (kstate_)
00703 robot_state::robotStateToRobotStateMsg(*kstate_, scene_msg.robot_state);
00704 else
00705 scene_msg.robot_state = moveit_msgs::RobotState();
00706
00707 if (acm_)
00708 acm_->getMessage(scene_msg.allowed_collision_matrix);
00709 else
00710 scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix();
00711
00712 if (active_collision_->crobot_)
00713 {
00714 active_collision_->crobot_->getPadding(scene_msg.link_padding);
00715 active_collision_->crobot_->getScale(scene_msg.link_scale);
00716 }
00717 else
00718 {
00719 scene_msg.link_padding.clear();
00720 scene_msg.link_scale.clear();
00721 }
00722
00723 scene_msg.object_colors.clear();
00724 if (object_colors_)
00725 {
00726 unsigned int i = 0;
00727 scene_msg.object_colors.resize(object_colors_->size());
00728 for (ObjectColorMap::const_iterator it = object_colors_->begin() ; it != object_colors_->end() ; ++it, ++i)
00729 {
00730 scene_msg.object_colors[i].id = it->first;
00731 scene_msg.object_colors[i].color = it->second;
00732 }
00733 }
00734
00735 scene_msg.world.collision_objects.clear();
00736 scene_msg.world.octomap = octomap_msgs::OctomapWithPose();
00737
00738 if (world_diff_)
00739 {
00740 bool do_omap = false;
00741 for (collision_detection::WorldDiff::const_iterator it = world_diff_->begin() ; it != world_diff_->end() ; ++it)
00742 {
00743 if (it->first == OCTOMAP_NS)
00744 do_omap = true;
00745 else if (it->second == collision_detection::World::DESTROY)
00746 {
00747 moveit_msgs::CollisionObject co;
00748 co.header.frame_id = getPlanningFrame();
00749 co.id = it->first;
00750 co.operation = moveit_msgs::CollisionObject::REMOVE;
00751 scene_msg.world.collision_objects.push_back(co);
00752 }
00753 else
00754 getPlanningSceneMsgCollisionObject(scene_msg, it->first);
00755 }
00756 if (do_omap)
00757 getPlanningSceneMsgOctomap(scene_msg);
00758 }
00759 }
00760
00761 namespace planning_scene
00762 {
00763 namespace
00764 {
00765 class ShapeVisitorAddToCollisionObject : public boost::static_visitor<void>
00766 {
00767 public:
00768
00769 ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject *obj) :
00770 boost::static_visitor<void>(), obj_(obj)
00771 {
00772 }
00773
00774 void setPoseMessage(const geometry_msgs::Pose *pose)
00775 {
00776 pose_ = pose;
00777 }
00778
00779 void operator()(const shape_msgs::Plane &shape_msg) const
00780 {
00781 obj_->planes.push_back(shape_msg);
00782 obj_->plane_poses.push_back(*pose_);
00783 }
00784
00785 void operator()(const shape_msgs::Mesh &shape_msg) const
00786 {
00787 obj_->meshes.push_back(shape_msg);
00788 obj_->mesh_poses.push_back(*pose_);
00789 }
00790
00791 void operator()(const shape_msgs::SolidPrimitive &shape_msg) const
00792 {
00793 obj_->primitives.push_back(shape_msg);
00794 obj_->primitive_poses.push_back(*pose_);
00795 }
00796
00797 private:
00798
00799 moveit_msgs::CollisionObject *obj_;
00800 const geometry_msgs::Pose *pose_;
00801 };
00802 }
00803 }
00804
00805 void planning_scene::PlanningScene::getPlanningSceneMsgCollisionObject(moveit_msgs::PlanningScene &scene_msg, const std::string &ns) const
00806 {
00807 moveit_msgs::CollisionObject co;
00808 co.header.frame_id = getPlanningFrame();
00809 co.id = ns;
00810 co.operation = moveit_msgs::CollisionObject::ADD;
00811 collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(ns);
00812 if (!obj)
00813 return;
00814 ShapeVisitorAddToCollisionObject sv(&co);
00815 for (std::size_t j = 0 ; j < obj->shapes_.size() ; ++j)
00816 {
00817 shapes::ShapeMsg sm;
00818 if (constructMsgFromShape(obj->shapes_[j].get(), sm))
00819 {
00820 geometry_msgs::Pose p;
00821 tf::poseEigenToMsg(obj->shape_poses_[j], p);
00822
00823 sv.setPoseMessage(&p);
00824 boost::apply_visitor(sv, sm);
00825 }
00826 }
00827
00828 if (!co.primitives.empty() || !co.meshes.empty() || !co.planes.empty())
00829 {
00830 if (hasObjectType(co.id))
00831 co.type = getObjectType(co.id);
00832 scene_msg.world.collision_objects.push_back(co);
00833 }
00834 }
00835
00836 void planning_scene::PlanningScene::getPlanningSceneMsgCollisionObjects(moveit_msgs::PlanningScene &scene_msg) const
00837 {
00838 scene_msg.world.collision_objects.clear();
00839 const std::vector<std::string> &ns = world_->getObjectIds();
00840 for (std::size_t i = 0 ; i < ns.size() ; ++i)
00841 if (ns[i] != OCTOMAP_NS)
00842 getPlanningSceneMsgCollisionObject(scene_msg, ns[i]);
00843 }
00844
00845 void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::PlanningScene &scene_msg) const
00846 {
00847 scene_msg.world.octomap.header.frame_id = getPlanningFrame();
00848 scene_msg.world.octomap.octomap = octomap_msgs::Octomap();
00849
00850 collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS);
00851 if (map)
00852 {
00853 if (map->shapes_.size() == 1)
00854 {
00855 const shapes::OcTree *o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
00856 octomap_msgs::fullMapToMsg(*o->octree, scene_msg.world.octomap.octomap);
00857 tf::poseEigenToMsg(map->shape_poses_[0], scene_msg.world.octomap.origin);
00858 }
00859 else
00860 logError("Unexpected number of shapes in octomap collision object. Not including '%s' object", OCTOMAP_NS.c_str());
00861 }
00862 }
00863
00864 void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene &scene_msg) const
00865 {
00866 scene_msg.name = name_;
00867 scene_msg.is_diff = false;
00868 scene_msg.robot_model_name = getRobotModel()->getName();
00869 getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
00870
00871 robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state);
00872 getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
00873 getCollisionRobot()->getPadding(scene_msg.link_padding);
00874 getCollisionRobot()->getScale(scene_msg.link_scale);
00875
00876 getPlanningSceneMsgObjectColors(scene_msg);
00877
00878
00879 getPlanningSceneMsgCollisionObjects(scene_msg);
00880
00881
00882 getPlanningSceneMsgOctomap(scene_msg);
00883 }
00884
00885 void planning_scene::PlanningScene::getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene &scene_msg) const
00886 {
00887 scene_msg.object_colors.clear();
00888
00889 unsigned int i = 0;
00890 ObjectColorMap cmap;
00891 getKnownObjectColors(cmap);
00892 scene_msg.object_colors.resize(cmap.size());
00893 for (ObjectColorMap::const_iterator it = cmap.begin() ; it != cmap.end() ; ++it, ++i)
00894 {
00895 scene_msg.object_colors[i].id = it->first;
00896 scene_msg.object_colors[i].color = it->second;
00897 }
00898 }
00899
00900 void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene &scene_msg, const moveit_msgs::PlanningSceneComponents &comp) const
00901 {
00902 scene_msg.is_diff = false;
00903 if (comp.components & moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS)
00904 {
00905 scene_msg.name = name_;
00906 scene_msg.robot_model_name = getRobotModel()->getName();
00907 }
00908
00909 if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
00910 getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
00911
00912 if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
00913 robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true);
00914 else
00915 if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
00916 robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false);
00917
00918 if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
00919 getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);
00920
00921 if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
00922 {
00923 getCollisionRobot()->getPadding(scene_msg.link_padding);
00924 getCollisionRobot()->getScale(scene_msg.link_scale);
00925 }
00926
00927 if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS)
00928 getPlanningSceneMsgObjectColors(scene_msg);
00929
00930
00931 if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
00932 getPlanningSceneMsgCollisionObjects(scene_msg);
00933 else
00934 if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES)
00935 {
00936 const std::vector<std::string> &ns = world_->getObjectIds();
00937 scene_msg.world.collision_objects.clear();
00938 scene_msg.world.collision_objects.reserve(ns.size());
00939 for (std::size_t i = 0 ; i < ns.size() ; ++i)
00940 if (ns[i] != OCTOMAP_NS)
00941 {
00942 moveit_msgs::CollisionObject co;
00943 co.id = ns[i];
00944 if (hasObjectType(co.id))
00945 co.type = getObjectType(co.id);
00946 scene_msg.world.collision_objects.push_back(co);
00947 }
00948 }
00949
00950
00951 if (comp.components & moveit_msgs::PlanningSceneComponents::OCTOMAP)
00952 getPlanningSceneMsgOctomap(scene_msg);
00953 }
00954
00955 void planning_scene::PlanningScene::saveGeometryToStream(std::ostream &out) const
00956 {
00957 out << name_ << std::endl;
00958 const std::vector<std::string> &ns = world_->getObjectIds();
00959 for (std::size_t i = 0 ; i < ns.size() ; ++i)
00960 if (ns[i] != OCTOMAP_NS)
00961 {
00962 collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(ns[i]);
00963 if (obj)
00964 {
00965 out << "* " << ns[i] << std::endl;
00966 out << obj->shapes_.size() << std::endl;
00967 for (std::size_t j = 0 ; j < obj->shapes_.size() ; ++j)
00968 {
00969 shapes::saveAsText(obj->shapes_[j].get(), out);
00970 out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl;
00971 Eigen::Quaterniond r(obj->shape_poses_[j].rotation());
00972 out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
00973 if (hasObjectColor(ns[i]))
00974 {
00975 const std_msgs::ColorRGBA &c = getObjectColor(ns[i]);
00976 out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl;
00977 }
00978 else
00979 out << "0 0 0 0" << std::endl;
00980 }
00981 }
00982 }
00983 out << "." << std::endl;
00984 }
00985
00986 void planning_scene::PlanningScene::loadGeometryFromStream(std::istream &in)
00987 {
00988 if (!in.good() || in.eof())
00989 return;
00990 std::getline(in, name_);
00991 do
00992 {
00993 std::string marker;
00994 in >> marker;
00995 if (!in.good() || in.eof())
00996 return;
00997 if (marker == "*")
00998 {
00999 std::string ns;
01000 std::getline(in, ns);
01001 if (!in.good() || in.eof())
01002 return;
01003 boost::algorithm::trim(ns);
01004 unsigned int shape_count;
01005 in >> shape_count;
01006 for (std::size_t i = 0 ; i < shape_count && in.good() && !in.eof() ; ++i)
01007 {
01008 shapes::Shape *s = shapes::constructShapeFromText(in);
01009 double x, y, z, rx, ry, rz, rw;
01010 in >> x >> y >> z;
01011 in >> rx >> ry >> rz >> rw;
01012 float r, g, b, a;
01013 in >> r >> g >> b >> a;
01014 if (s)
01015 {
01016 Eigen::Affine3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
01017 world_->addToObject(ns, shapes::ShapePtr(s), pose);
01018 if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
01019 {
01020 std_msgs::ColorRGBA color;
01021 color.r = r; color.g = g; color.b = b; color.a = a;
01022 setObjectColor(ns, color);
01023 }
01024 }
01025 }
01026 }
01027 else
01028 break;
01029 } while (true);
01030 }
01031
01032 void planning_scene::PlanningScene::setCurrentState(const moveit_msgs::RobotState &state)
01033 {
01034 if (parent_)
01035 {
01036 if (!kstate_)
01037 {
01038 kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
01039 kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
01040 }
01041 robot_state::robotStateMsgToRobotState(getTransforms(), state, *kstate_);
01042 }
01043 else
01044 robot_state::robotStateMsgToRobotState(*ftf_, state, *kstate_);
01045
01046
01047 for (std::size_t i = 0 ; i < state.attached_collision_objects.size() ; ++i)
01048 {
01049 const moveit_msgs::CollisionObject &o = state.attached_collision_objects[i].object;
01050 if (!o.id.empty() && o.operation == moveit_msgs::CollisionObject::ADD && (!o.type.db.empty() || !o.type.key.empty()))
01051 setObjectType(o.id, o.type);
01052 }
01053 }
01054
01055 void planning_scene::PlanningScene::setCurrentState(const robot_state::RobotState &state)
01056 {
01057 getCurrentStateNonConst() = state;
01058 }
01059
01060 void planning_scene::PlanningScene::decoupleParent()
01061 {
01062 if (!parent_)
01063 return;
01064
01065 if (!ftf_)
01066 {
01067 ftf_.reset(new SceneTransforms(this));
01068 ftf_->setAllTransforms(parent_->getTransforms().getAllTransforms());
01069 }
01070
01071 if (!kstate_)
01072 {
01073 kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
01074 kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
01075 }
01076
01077 if (!acm_)
01078 acm_.reset(new collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix()));
01079
01080 for (CollisionDetectorIterator it = collision_.begin() ; it != collision_.end() ; ++it)
01081 {
01082 if (!it->second->crobot_)
01083 {
01084 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
01085 it->second->crobot_const_ = it->second->crobot_;
01086 }
01087 if (!it->second->crobot_unpadded_)
01088 {
01089 it->second->crobot_unpadded_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobotUnpadded());
01090 it->second->crobot_unpadded_const_ = it->second->crobot_unpadded_;
01091 }
01092 it->second->parent_.reset();
01093 }
01094 world_diff_.reset();
01095
01096 if (!object_colors_)
01097 {
01098 ObjectColorMap kc;
01099 parent_->getKnownObjectColors(kc);
01100 object_colors_.reset(new ObjectColorMap(kc));
01101 }
01102 else
01103 {
01104 ObjectColorMap kc;
01105 parent_->getKnownObjectColors(kc);
01106 for (ObjectColorMap::const_iterator it = kc.begin() ; it != kc.end() ; ++it)
01107 if (object_colors_->find(it->first) == object_colors_->end())
01108 (*object_colors_)[it->first] = it->second;
01109 }
01110
01111 if (!object_types_)
01112 {
01113 ObjectTypeMap kc;
01114 parent_->getKnownObjectTypes(kc);
01115 object_types_.reset(new ObjectTypeMap(kc));
01116 }
01117 else
01118 {
01119 ObjectTypeMap kc;
01120 parent_->getKnownObjectTypes(kc);
01121 for (ObjectTypeMap::const_iterator it = kc.begin() ; it != kc.end() ; ++it)
01122 if (object_types_->find(it->first) == object_types_->end())
01123 (*object_types_)[it->first] = it->second;
01124 }
01125
01126 parent_.reset();
01127 }
01128
01129 void planning_scene::PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene_msg)
01130 {
01131 logDebug("moveit.planning_scene: Adding planning scene diff");
01132 if (!scene_msg.name.empty())
01133 name_ = scene_msg.name;
01134
01135 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
01136 logWarn("Setting the scene for model '%s' but model '%s' is loaded.", scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
01137
01138
01139
01140 if (!scene_msg.fixed_frame_transforms.empty())
01141 {
01142 if (!ftf_)
01143 ftf_.reset(new SceneTransforms(this));
01144 ftf_->setTransforms(scene_msg.fixed_frame_transforms);
01145 }
01146
01147
01148 if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
01149 !scene_msg.robot_state.joint_state.name.empty() ||
01150 !scene_msg.robot_state.attached_collision_objects.empty())
01151 setCurrentState(scene_msg.robot_state);
01152
01153
01154 if (!scene_msg.allowed_collision_matrix.entry_names.empty())
01155 acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
01156
01157 if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
01158 {
01159 for (CollisionDetectorIterator it = collision_.begin() ; it != collision_.end() ; ++it)
01160 {
01161 if (!it->second->crobot_)
01162 {
01163 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
01164 it->second->crobot_const_ = it->second->crobot_;
01165 }
01166 it->second->crobot_->setPadding(scene_msg.link_padding);
01167 it->second->crobot_->setScale(scene_msg.link_scale);
01168 }
01169 }
01170
01171
01172 if (!scene_msg.object_colors.empty())
01173 {
01174 object_colors_.reset();
01175 for (std::size_t i = 0 ; i < scene_msg.object_colors.size() ; ++i)
01176 setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
01177 }
01178
01179
01180 for (std::size_t i = 0 ; i < scene_msg.world.collision_objects.size() ; ++i)
01181 processCollisionObjectMsg(scene_msg.world.collision_objects[i]);
01182
01183
01184 if (!scene_msg.world.octomap.octomap.data.empty())
01185 processOctomapMsg(scene_msg.world.octomap);
01186 }
01187
01188 void planning_scene::PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
01189 {
01190 logDebug("moveit.planning_scene: Setting new planning scene: '%s'", scene_msg.name.c_str());
01191 name_ = scene_msg.name;
01192
01193 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName())
01194 logWarn("Setting the scene for model '%s' but model '%s' is loaded.", scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str());
01195
01196 if (parent_)
01197 decoupleParent();
01198
01199 object_types_.reset();
01200 ftf_->setTransforms(scene_msg.fixed_frame_transforms);
01201 setCurrentState(scene_msg.robot_state);
01202 acm_.reset(new collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix));
01203 for (CollisionDetectorIterator it = collision_.begin() ; it != collision_.end() ; ++it)
01204 {
01205 if (!it->second->crobot_)
01206 {
01207 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
01208 it->second->crobot_const_ = it->second->crobot_;
01209 }
01210 it->second->crobot_->setPadding(scene_msg.link_padding);
01211 it->second->crobot_->setScale(scene_msg.link_scale);
01212 }
01213 object_colors_.reset(new ObjectColorMap());
01214 for (std::size_t i = 0 ; i < scene_msg.object_colors.size() ; ++i)
01215 setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
01216 world_->clearObjects();
01217 processPlanningSceneWorldMsg(scene_msg.world);
01218 }
01219
01220 void planning_scene::PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
01221 {
01222 for (std::size_t i = 0 ; i < world.collision_objects.size() ; ++i)
01223 processCollisionObjectMsg(world.collision_objects[i]);
01224 processOctomapMsg(world.octomap);
01225 }
01226
01227 void planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
01228 {
01229 if (scene_msg.is_diff)
01230 setPlanningSceneDiffMsg(scene_msg);
01231 else
01232 setPlanningSceneMsg(scene_msg);
01233 }
01234
01235 void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map)
01236 {
01237
01238 world_->removeObject(OCTOMAP_NS);
01239
01240 if (map.data.empty())
01241 return;
01242
01243 if (map.id != "OcTree")
01244 {
01245 logError("Received ocomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
01246 return;
01247 }
01248
01249 boost::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map)));
01250 if (!map.header.frame_id.empty())
01251 {
01252 const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id);
01253 world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
01254 }
01255 else
01256 {
01257 world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Affine3d::Identity());
01258 }
01259 }
01260
01261 void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
01262 {
01263
01264 world_->removeObject(OCTOMAP_NS);
01265
01266 if (map.octomap.data.empty())
01267 return;
01268
01269 if (map.octomap.id != "OcTree")
01270 {
01271 logError("Received ocomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
01272 return;
01273 }
01274
01275 boost::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
01276 const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id);
01277 Eigen::Affine3d p;
01278 tf::poseMsgToEigen(map.origin, p);
01279 p = t * p;
01280 world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
01281 }
01282
01283 void planning_scene::PlanningScene::processOctomapPtr(const boost::shared_ptr<const octomap::OcTree> &octree, const Eigen::Affine3d &t)
01284 {
01285 collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS);
01286 if (map)
01287 {
01288 if (map->shapes_.size() == 1)
01289 {
01290
01291 const shapes::OcTree *o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
01292 if (o->octree == octree)
01293 {
01294
01295 if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
01296 {
01297 if (world_diff_)
01298 world_diff_->set(OCTOMAP_NS,
01299 collision_detection::World::DESTROY |
01300 collision_detection::World::CREATE |
01301 collision_detection::World::ADD_SHAPE);
01302 }
01303 else
01304 {
01305 shapes::ShapeConstPtr shape = map->shapes_[0];
01306 map.reset();
01307 world_->moveShapeInObject(OCTOMAP_NS, shape, t);
01308 }
01309 return;
01310 }
01311 }
01312 }
01313
01314 world_->removeObject(OCTOMAP_NS);
01315 world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t);
01316 }
01317
01318 bool planning_scene::PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
01319 {
01320 if (object.object.operation == moveit_msgs::CollisionObject::ADD && !getRobotModel()->hasLinkModel(object.link_name))
01321 {
01322 logError("Unable to attach a body to link '%s' (link not found)", object.link_name.c_str());
01323 return false;
01324 }
01325
01326 if (object.object.id == OCTOMAP_NS)
01327 {
01328 logError("The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
01329 return false;
01330 }
01331
01332 if (!kstate_)
01333 {
01334 kstate_.reset(new robot_state::RobotState(parent_->getCurrentState()));
01335 kstate_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
01336 }
01337 kstate_->update();
01338
01339 if (object.object.operation == moveit_msgs::CollisionObject::ADD || object.object.operation == moveit_msgs::CollisionObject::APPEND)
01340 {
01341 if (object.object.primitives.size() != object.object.primitive_poses.size())
01342 {
01343 logError("Number of primitive shapes does not match number of poses in attached collision object message");
01344 return false;
01345 }
01346
01347 if (object.object.meshes.size() != object.object.mesh_poses.size())
01348 {
01349 logError("Number of meshes does not match number of poses in attached collision object message");
01350 return false;
01351 }
01352
01353 if (object.object.planes.size() != object.object.plane_poses.size())
01354 {
01355 logError("Number of planes does not match number of poses in attached collision object message");
01356 return false;
01357 }
01358
01359 const robot_model::LinkModel *lm = getRobotModel()->getLinkModel(object.link_name);
01360 if (lm)
01361 {
01362 std::vector<shapes::ShapeConstPtr> shapes;
01363 EigenSTL::vector_Affine3d poses;
01364
01365
01366 if (object.object.operation == moveit_msgs::CollisionObject::ADD &&
01367 object.object.primitives.empty() && object.object.meshes.empty() && object.object.planes.empty())
01368 {
01369 collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(object.object.id);
01370 if (obj)
01371 {
01372 logInform("Attaching world object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str());
01373
01374
01375 shapes = obj->shapes_;
01376 poses = obj->shape_poses_;
01377
01378 world_->removeObject(object.object.id);
01379
01380
01381 const Eigen::Affine3d &i_t = kstate_->getGlobalLinkTransform(lm).inverse();
01382 for (std::size_t i = 0 ; i < poses.size() ; ++i)
01383 poses[i] = i_t * poses[i];
01384 }
01385 else
01386 {
01387 logError("Attempting to attach object '%s' to link '%s' but no geometry specified and such an object does not exist in the collision world",
01388 object.object.id.c_str(), object.link_name.c_str());
01389 return false;
01390 }
01391 }
01392 else
01393 {
01394
01395 if (world_->removeObject(object.object.id))
01396 {
01397 if (object.object.operation == moveit_msgs::CollisionObject::ADD)
01398 logInform("Removing world object with the same name as newly attached object: '%s'", object.object.id.c_str());
01399 else
01400 logWarn("You tried to append geometry to an attached object that is actually a world object ('%s'). World geometry is ignored.", object.object.id.c_str());
01401 }
01402
01403 for (std::size_t i = 0 ; i < object.object.primitives.size() ; ++i)
01404 {
01405 shapes::Shape *s = shapes::constructShapeFromMsg(object.object.primitives[i]);
01406 if (s)
01407 {
01408 Eigen::Affine3d p;
01409 tf::poseMsgToEigen(object.object.primitive_poses[i], p);
01410 shapes.push_back(shapes::ShapeConstPtr(s));
01411 poses.push_back(p);
01412 }
01413 }
01414 for (std::size_t i = 0 ; i < object.object.meshes.size() ; ++i)
01415 {
01416 shapes::Shape *s = shapes::constructShapeFromMsg(object.object.meshes[i]);
01417 if (s)
01418 {
01419 Eigen::Affine3d p;
01420 tf::poseMsgToEigen(object.object.mesh_poses[i], p);
01421 shapes.push_back(shapes::ShapeConstPtr(s));
01422 poses.push_back(p);
01423 }
01424 }
01425 for (std::size_t i = 0 ; i < object.object.planes.size() ; ++i)
01426 {
01427 shapes::Shape *s = shapes::constructShapeFromMsg(object.object.planes[i]);
01428 if (s)
01429 {
01430 Eigen::Affine3d p;
01431 tf::poseMsgToEigen(object.object.plane_poses[i], p);
01432 shapes.push_back(shapes::ShapeConstPtr(s));
01433 poses.push_back(p);
01434 }
01435 }
01436
01437
01438 if (object.object.header.frame_id != object.link_name)
01439 {
01440 const Eigen::Affine3d &t = kstate_->getGlobalLinkTransform(lm).inverse() * getTransforms().getTransform(object.object.header.frame_id);
01441 for (std::size_t i = 0 ; i < poses.size() ; ++i)
01442 poses[i] = t * poses[i];
01443 }
01444 }
01445
01446 if (shapes.empty())
01447 {
01448 logError("There is no geometry to attach to link '%s' as part of attached body '%s'", object.link_name.c_str(), object.object.id.c_str());
01449 return false;
01450 }
01451
01452 if (!object.object.type.db.empty() || !object.object.type.key.empty())
01453 setObjectType(object.object.id, object.object.type);
01454
01455 if (object.object.operation == moveit_msgs::CollisionObject::ADD || !kstate_->hasAttachedBody(object.object.id))
01456 {
01457
01458 if (kstate_->clearAttachedBody(object.object.id))
01459 logInform("The robot state already had an object named '%s' attached to link '%s'. The object was replaced.",
01460 object.object.id.c_str(), object.link_name.c_str());
01461 kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, object.detach_posture);
01462 logInform("Attached object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str());
01463 }
01464 else
01465 {
01466 const robot_state::AttachedBody *ab = kstate_->getAttachedBody(object.object.id);
01467 shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end());
01468 poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end());
01469 trajectory_msgs::JointTrajectory detach_posture = object.detach_posture.joint_names.empty() ? ab->getDetachPosture() : object.detach_posture;
01470 std::set<std::string> ab_touch_links = ab->getTouchLinks();
01471 kstate_->clearAttachedBody(object.object.id);
01472 if (object.touch_links.empty())
01473 kstate_->attachBody(object.object.id, shapes, poses, ab_touch_links, object.link_name, detach_posture);
01474 else
01475 kstate_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, detach_posture);
01476 logInform("Added shapes to object '%s' attached to link '%s'", object.object.id.c_str(), object.link_name.c_str());
01477 }
01478
01479 return true;
01480 }
01481 else
01482 logError("Robot state is not compatible with robot model. This could be fatal.");
01483 }
01484 else if (object.object.operation == moveit_msgs::CollisionObject::REMOVE)
01485 {
01486 std::vector<const robot_state::AttachedBody*> attached_bodies;
01487 if (object.link_name.empty())
01488 {
01489 if (object.object.id.empty())
01490 kstate_->getAttachedBodies(attached_bodies);
01491 else
01492 {
01493 const robot_state::AttachedBody *ab = kstate_->getAttachedBody(object.object.id);
01494 if (ab)
01495 attached_bodies.push_back(ab);
01496 }
01497 }
01498 else
01499 {
01500 const robot_model::LinkModel *lm = getRobotModel()->getLinkModel(object.link_name);
01501 if (lm)
01502 {
01503 if (object.object.id.empty())
01504 {
01505 kstate_->getAttachedBodies(attached_bodies, lm);
01506 }
01507 else
01508 {
01509 const robot_state::AttachedBody *ab = kstate_->getAttachedBody(object.object.id);
01510 if (ab)
01511 attached_bodies.push_back(ab);
01512 }
01513 }
01514 }
01515
01516 for (std::size_t i = 0; i < attached_bodies.size(); ++i)
01517 {
01518 std::vector<shapes::ShapeConstPtr> shapes = attached_bodies[i]->getShapes();
01519 EigenSTL::vector_Affine3d poses = attached_bodies[i]->getGlobalCollisionBodyTransforms();
01520 std::string name = attached_bodies[i]->getName();
01521
01522 kstate_->clearAttachedBody(name);
01523
01524 if (world_->hasObject(name))
01525 logWarn("The collision world already has an object with the same name as the body about to be detached. NOT adding the detached body '%s' to the collision world.", object.object.id.c_str());
01526 else
01527 {
01528 world_->addToObject(name, shapes, poses);
01529 logInform("Detached object '%s' from link '%s' and added it back in the collision world", name.c_str(), object.link_name.c_str());
01530 }
01531 }
01532 if (!attached_bodies.empty() || object.object.id.empty())
01533 return true;
01534 }
01535 else if (object.object.operation == moveit_msgs::CollisionObject::MOVE)
01536 {
01537 logError("Move for attached objects not yet implemented");
01538 }
01539 else
01540 {
01541 logError("Unknown collision object operation: %d", object.object.operation);
01542 }
01543
01544 return false;
01545 }
01546
01547 bool planning_scene::PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
01548 {
01549 if (object.id == OCTOMAP_NS)
01550 {
01551 logError("The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str());
01552 return false;
01553 }
01554
01555 if (object.operation == moveit_msgs::CollisionObject::ADD || object.operation == moveit_msgs::CollisionObject::APPEND)
01556 {
01557 if (object.primitives.empty() && object.meshes.empty() && object.planes.empty())
01558 {
01559 logError("There are no shapes specified in the collision object message");
01560 return false;
01561 }
01562
01563 if (object.primitives.size() != object.primitive_poses.size())
01564 {
01565 logError("Number of primitive shapes does not match number of poses in collision object message");
01566 return false;
01567 }
01568
01569 if (object.meshes.size() != object.mesh_poses.size())
01570 {
01571 logError("Number of meshes does not match number of poses in collision object message");
01572 return false;
01573 }
01574
01575 if (object.planes.size() != object.plane_poses.size())
01576 {
01577 logError("Number of planes does not match number of poses in collision object message");
01578 return false;
01579 }
01580
01581
01582 if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
01583 world_->removeObject(object.id);
01584
01585 const Eigen::Affine3d &t = getTransforms().getTransform(object.header.frame_id);
01586
01587 for (std::size_t i = 0 ; i < object.primitives.size() ; ++i)
01588 {
01589 shapes::Shape *s = shapes::constructShapeFromMsg(object.primitives[i]);
01590 if (s)
01591 {
01592 Eigen::Affine3d p;
01593 tf::poseMsgToEigen(object.primitive_poses[i], p);
01594 world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
01595 }
01596 }
01597 for (std::size_t i = 0 ; i < object.meshes.size() ; ++i)
01598 {
01599 shapes::Shape *s = shapes::constructShapeFromMsg(object.meshes[i]);
01600 if (s)
01601 {
01602 Eigen::Affine3d p;
01603 tf::poseMsgToEigen(object.mesh_poses[i], p);
01604 world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
01605 }
01606 }
01607 for (std::size_t i = 0 ; i < object.planes.size() ; ++i)
01608 {
01609 shapes::Shape *s = shapes::constructShapeFromMsg(object.planes[i]);
01610 if (s)
01611 {
01612 Eigen::Affine3d p;
01613 tf::poseMsgToEigen(object.plane_poses[i], p);
01614 world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
01615 }
01616 }
01617 if (!object.type.key.empty() || !object.type.db.empty())
01618 setObjectType(object.id, object.type);
01619 return true;
01620 }
01621 else if (object.operation == moveit_msgs::CollisionObject::REMOVE)
01622 {
01623 if (object.id.empty())
01624 {
01625 const std::vector<std::string> &object_ids = world_->getObjectIds();
01626 for (std::size_t i = 0; i < object_ids.size(); ++i)
01627 if (object_ids[i] != OCTOMAP_NS)
01628 world_->removeObject(object_ids[i]);
01629 }
01630 else
01631 world_->removeObject(object.id);
01632 return true;
01633 }
01634 else if (object.operation == moveit_msgs::CollisionObject::MOVE)
01635 {
01636 if (world_->hasObject(object.id))
01637 {
01638 if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty())
01639 logWarn("Move operation for object '%s' ignores the geometry specified in the message.", object.id.c_str());
01640
01641 const Eigen::Affine3d &t = getTransforms().getTransform(object.header.frame_id);
01642 EigenSTL::vector_Affine3d new_poses;
01643 for (std::size_t i = 0 ; i < object.primitive_poses.size() ; ++i)
01644 {
01645 Eigen::Affine3d p;
01646 tf::poseMsgToEigen(object.primitive_poses[i], p);
01647 new_poses.push_back(t * p);
01648 }
01649 for (std::size_t i = 0 ; i < object.mesh_poses.size() ; ++i)
01650 {
01651 Eigen::Affine3d p;
01652 tf::poseMsgToEigen(object.mesh_poses[i], p);
01653 new_poses.push_back(t * p);
01654 }
01655 for (std::size_t i = 0 ; i < object.plane_poses.size() ; ++i)
01656 {
01657 Eigen::Affine3d p;
01658 tf::poseMsgToEigen(object.plane_poses[i], p);
01659 new_poses.push_back(t * p);
01660 }
01661
01662 collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id);
01663 if (obj->shapes_.size() == new_poses.size())
01664 {
01665 std::vector<shapes::ShapeConstPtr> shapes = obj->shapes_;
01666 obj.reset();
01667 world_->removeObject(object.id);
01668 world_->addToObject(object.id, shapes, new_poses);
01669 }
01670 else
01671 {
01672 logError("Number of supplied poses (%u) for object '%s' does not match number of shapes (%u). Not moving.",
01673 (unsigned int)new_poses.size(), object.id.c_str(), (unsigned int)obj->shapes_.size());
01674 return false;
01675 }
01676 return true;
01677 }
01678 else
01679 logError("World object '%s' does not exist. Cannot move.", object.id.c_str());
01680 }
01681 else
01682 logError("Unknown collision object operation: %d", object.operation);
01683 return false;
01684 }
01685
01686 const Eigen::Affine3d& planning_scene::PlanningScene::getFrameTransform(const std::string &id) const
01687 {
01688 return getFrameTransform(getCurrentState(), id);
01689 }
01690
01691 const Eigen::Affine3d& planning_scene::PlanningScene::getFrameTransform(const std::string &id)
01692 {
01693 if (getCurrentState().dirtyLinkTransforms())
01694 return getFrameTransform(getCurrentStateNonConst(), id);
01695 else
01696 return getFrameTransform(getCurrentState(), id);
01697 }
01698
01699 const Eigen::Affine3d& planning_scene::PlanningScene::getFrameTransform(const robot_state::RobotState &state, const std::string &id) const
01700 {
01701 if (!id.empty() && id[0] == '/')
01702 return getFrameTransform(id.substr(1));
01703 if (state.knowsFrameTransform(id))
01704 return state.getFrameTransform(id);
01705 if (getWorld()->hasObject(id))
01706 {
01707 collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
01708 if (obj->shape_poses_.size() > 1)
01709 {
01710 logWarn("More than one shapes in object '%s'. Using first one to decide transform");
01711 return obj->shape_poses_[0];
01712 }
01713 else
01714 if (obj->shape_poses_.size() == 1)
01715 return obj->shape_poses_[0];
01716 }
01717 return getTransforms().Transforms::getTransform(id);
01718 }
01719
01720 bool planning_scene::PlanningScene::knowsFrameTransform(const std::string &id) const
01721 {
01722 return knowsFrameTransform(getCurrentState(), id);
01723 }
01724
01725 bool planning_scene::PlanningScene::knowsFrameTransform(const robot_state::RobotState &state, const std::string &id) const
01726 {
01727 if (!id.empty() && id[0] == '/')
01728 return knowsFrameTransform(id.substr(1));
01729 if (state.knowsFrameTransform(id))
01730 return true;
01731 if (getWorld()->hasObject(id))
01732 {
01733 collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id);
01734 return obj->shape_poses_.size() == 1;
01735 }
01736 return getTransforms().Transforms::canTransform(id);
01737 }
01738
01739 bool planning_scene::PlanningScene::hasObjectType(const std::string &id) const
01740 {
01741 if (object_types_)
01742 if (object_types_->find(id) != object_types_->end())
01743 return true;
01744 if (parent_)
01745 return parent_->hasObjectType(id);
01746 return false;
01747 }
01748
01749 const object_recognition_msgs::ObjectType& planning_scene::PlanningScene::getObjectType(const std::string &id) const
01750 {
01751 if (object_types_)
01752 {
01753 ObjectTypeMap::const_iterator it = object_types_->find(id);
01754 if (it != object_types_->end())
01755 return it->second;
01756 }
01757 if (parent_)
01758 return parent_->getObjectType(id);
01759 static const object_recognition_msgs::ObjectType empty;
01760 return empty;
01761
01762 }
01763
01764 void planning_scene::PlanningScene::setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
01765 {
01766 if (!object_types_)
01767 object_types_.reset(new ObjectTypeMap());
01768 (*object_types_)[id] = type;
01769 }
01770
01771 void planning_scene::PlanningScene::removeObjectType(const std::string &id)
01772 {
01773 if (object_types_)
01774 object_types_->erase(id);
01775 }
01776
01777 void planning_scene::PlanningScene::getKnownObjectTypes(ObjectTypeMap &kc) const
01778 {
01779 kc.clear();
01780 if (parent_)
01781 parent_->getKnownObjectTypes(kc);
01782 if (object_types_)
01783 for (ObjectTypeMap::const_iterator it = object_types_->begin() ; it != object_types_->end() ; ++it)
01784 kc[it->first] = it->second;
01785 }
01786
01787 bool planning_scene::PlanningScene::hasObjectColor(const std::string &id) const
01788 {
01789 if (object_colors_)
01790 if (object_colors_->find(id) != object_colors_->end())
01791 return true;
01792 if (parent_)
01793 return parent_->hasObjectColor(id);
01794 return false;
01795 }
01796
01797 const std_msgs::ColorRGBA& planning_scene::PlanningScene::getObjectColor(const std::string &id) const
01798 {
01799 if (object_colors_)
01800 {
01801 ObjectColorMap::const_iterator it = object_colors_->find(id);
01802 if (it != object_colors_->end())
01803 return it->second;
01804 }
01805 if (parent_)
01806 return parent_->getObjectColor(id);
01807 static const std_msgs::ColorRGBA empty;
01808 return empty;
01809 }
01810
01811 void planning_scene::PlanningScene::getKnownObjectColors(ObjectColorMap &kc) const
01812 {
01813 kc.clear();
01814 if (parent_)
01815 parent_->getKnownObjectColors(kc);
01816 if (object_colors_)
01817 for (ObjectColorMap::const_iterator it = object_colors_->begin() ; it != object_colors_->end() ; ++it)
01818 kc[it->first] = it->second;
01819 }
01820
01821 void planning_scene::PlanningScene::setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
01822 {
01823 if (!object_colors_)
01824 object_colors_.reset(new ObjectColorMap());
01825 (*object_colors_)[id] = color;
01826 }
01827
01828 void planning_scene::PlanningScene::removeObjectColor(const std::string &id)
01829 {
01830 if (object_colors_)
01831 object_colors_->erase(id);
01832 }
01833
01834 bool planning_scene::PlanningScene::isStateColliding(const moveit_msgs::RobotState &state, const std::string &group, bool verbose) const
01835 {
01836 robot_state::RobotState s(getCurrentState());
01837 robot_state::robotStateMsgToRobotState(getTransforms(), state, s);
01838 return isStateColliding(s, group, verbose);
01839 }
01840
01841 bool planning_scene::PlanningScene::isStateColliding(const std::string &group, bool verbose)
01842 {
01843 if (getCurrentState().dirtyCollisionBodyTransforms())
01844 return isStateColliding(getCurrentStateNonConst(), group, verbose);
01845 else
01846 return isStateColliding(getCurrentState(), group, verbose);
01847 }
01848
01849 bool planning_scene::PlanningScene::isStateColliding(const robot_state::RobotState &state, const std::string &group, bool verbose) const
01850 {
01851 collision_detection::CollisionRequest req;
01852 req.verbose = verbose;
01853 req.group_name = group;
01854 collision_detection::CollisionResult res;
01855 checkCollision(req, res, state);
01856 return res.collision;
01857 }
01858
01859 bool planning_scene::PlanningScene::isStateFeasible(const moveit_msgs::RobotState &state, bool verbose) const
01860 {
01861 if (state_feasibility_)
01862 {
01863 robot_state::RobotState s(getCurrentState());
01864 robot_state::robotStateMsgToRobotState(getTransforms(), state, s);
01865 return state_feasibility_(s, verbose);
01866 }
01867 return true;
01868 }
01869
01870 bool planning_scene::PlanningScene::isStateFeasible(const robot_state::RobotState &state, bool verbose) const
01871 {
01872 if (state_feasibility_)
01873 return state_feasibility_(state, verbose);
01874 return true;
01875 }
01876
01877 bool planning_scene::PlanningScene::isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose) const
01878 {
01879 robot_state::RobotState s(getCurrentState());
01880 robot_state::robotStateMsgToRobotState(getTransforms(), state, s);
01881 return isStateConstrained(s, constr, verbose);
01882 }
01883
01884
01885 bool planning_scene::PlanningScene::isStateConstrained(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose) const
01886 {
01887 kinematic_constraints::KinematicConstraintSetPtr ks(new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
01888 ks->add(constr, getTransforms());
01889 if (ks->empty())
01890 return true;
01891 else
01892 return isStateConstrained(state, *ks, verbose);
01893 }
01894
01895 bool planning_scene::PlanningScene::isStateConstrained(const moveit_msgs::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, bool verbose) const
01896 {
01897 robot_state::RobotState s(getCurrentState());
01898 robot_state::robotStateMsgToRobotState(getTransforms(), state, s);
01899 return isStateConstrained(s, constr, verbose);
01900 }
01901
01902 bool planning_scene::PlanningScene::isStateConstrained(const robot_state::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, bool verbose) const
01903 {
01904 return constr.decide(state, verbose).satisfied;
01905 }
01906
01907 bool planning_scene::PlanningScene::isStateValid(const robot_state::RobotState &state, const std::string &group, bool verbose) const
01908 {
01909 static const moveit_msgs::Constraints emp_constraints;
01910 return isStateValid(state, emp_constraints, group, verbose);
01911 }
01912
01913 bool planning_scene::PlanningScene::isStateValid(const moveit_msgs::RobotState &state, const std::string &group, bool verbose) const
01914 {
01915 static const moveit_msgs::Constraints emp_constraints;
01916 return isStateValid(state, emp_constraints, group, verbose);
01917 }
01918
01919 bool planning_scene::PlanningScene::isStateValid(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group, bool verbose) const
01920 {
01921 robot_state::RobotState s(getCurrentState());
01922 robot_state::robotStateMsgToRobotState(getTransforms(), state, s);
01923 return isStateValid(s, constr, group, verbose);
01924 }
01925
01926 bool planning_scene::PlanningScene::isStateValid(const robot_state::RobotState &state, const moveit_msgs::Constraints &constr, const std::string &group, bool verbose) const
01927 {
01928 if (isStateColliding(state, group, verbose))
01929 return false;
01930 if (!isStateFeasible(state, verbose))
01931 return false;
01932 return isStateConstrained(state, constr, verbose);
01933 }
01934
01935 bool planning_scene::PlanningScene::isStateValid(const robot_state::RobotState &state, const kinematic_constraints::KinematicConstraintSet &constr, const std::string &group, bool verbose) const
01936 {
01937 if (isStateColliding(state, group, verbose))
01938 return false;
01939 if (!isStateFeasible(state, verbose))
01940 return false;
01941 return isStateConstrained(state, constr, verbose);
01942 }
01943
01944 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState &start_state,
01945 const moveit_msgs::RobotTrajectory &trajectory,
01946 const std::string &group, bool verbose,
01947 std::vector<std::size_t> *invalid_index) const
01948 {
01949 static const moveit_msgs::Constraints emp_constraints;
01950 static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
01951 return isPathValid(start_state, trajectory, emp_constraints, emp_constraints_vector, group, verbose, invalid_index);
01952 }
01953
01954 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState &start_state,
01955 const moveit_msgs::RobotTrajectory &trajectory,
01956 const moveit_msgs::Constraints& path_constraints,
01957 const std::string &group, bool verbose,
01958 std::vector<std::size_t> *invalid_index) const
01959 {
01960 static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
01961 return isPathValid(start_state, trajectory, path_constraints, emp_constraints_vector, group, verbose, invalid_index);
01962 }
01963
01964 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState &start_state,
01965 const moveit_msgs::RobotTrajectory &trajectory,
01966 const moveit_msgs::Constraints& path_constraints,
01967 const moveit_msgs::Constraints& goal_constraints,
01968 const std::string &group, bool verbose,
01969 std::vector<std::size_t> *invalid_index) const
01970 {
01971 std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
01972 return isPathValid(start_state, trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
01973 }
01974
01975 bool planning_scene::PlanningScene::isPathValid(const moveit_msgs::RobotState &start_state,
01976 const moveit_msgs::RobotTrajectory &trajectory,
01977 const moveit_msgs::Constraints& path_constraints,
01978 const std::vector<moveit_msgs::Constraints>& goal_constraints,
01979 const std::string &group, bool verbose,
01980 std::vector<std::size_t> *invalid_index) const
01981 {
01982 robot_trajectory::RobotTrajectory t(getRobotModel(), group);
01983 robot_state::RobotState start(getCurrentState());
01984 robot_state::robotStateMsgToRobotState(getTransforms(), start_state, start);
01985 t.setRobotTrajectoryMsg(start, trajectory);
01986 return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index);
01987 }
01988
01989 bool planning_scene::PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
01990 const moveit_msgs::Constraints& path_constraints,
01991 const std::vector<moveit_msgs::Constraints>& goal_constraints,
01992 const std::string &group, bool verbose, std::vector<std::size_t> *invalid_index) const
01993 {
01994 bool result = true;
01995 if (invalid_index)
01996 invalid_index->clear();
01997 kinematic_constraints::KinematicConstraintSet ks_p(getRobotModel());
01998 ks_p.add(path_constraints, getTransforms());
01999 std::size_t n_wp = trajectory.getWayPointCount();
02000 for (std::size_t i = 0 ; i < n_wp ; ++i)
02001 {
02002 const robot_state::RobotState &st = trajectory.getWayPoint(i);
02003
02004 bool this_state_valid = true;
02005 if (isStateColliding(st, group, verbose))
02006 this_state_valid = false;
02007 if (!isStateFeasible(st, verbose))
02008 this_state_valid = false;
02009 if (!ks_p.empty() && !ks_p.decide(st, verbose).satisfied)
02010 this_state_valid = false;
02011
02012 if (!this_state_valid)
02013 {
02014 if (invalid_index)
02015 invalid_index->push_back(i);
02016 else
02017 return false;
02018 result = false;
02019 }
02020
02021
02022 if (i + 1 == n_wp && !goal_constraints.empty())
02023 {
02024 bool found = false;
02025 for (std::size_t k = 0 ; k < goal_constraints.size() ; ++k)
02026 {
02027 if (isStateConstrained(st, goal_constraints[k]))
02028 {
02029 found = true;
02030 break;
02031 }
02032 }
02033 if (!found)
02034 {
02035 if (verbose)
02036 logInform("Goal not satisfied");
02037 if (invalid_index)
02038 invalid_index->push_back(i);
02039 result = false;
02040 }
02041 }
02042 }
02043 return result;
02044 }
02045
02046
02047 bool planning_scene::PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
02048 const moveit_msgs::Constraints& path_constraints,
02049 const moveit_msgs::Constraints& goal_constraints,
02050 const std::string &group, bool verbose, std::vector<std::size_t> *invalid_index) const
02051 {
02052 std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
02053 return isPathValid(trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
02054 }
02055
02056 bool planning_scene::PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
02057 const moveit_msgs::Constraints& path_constraints,
02058 const std::string &group, bool verbose, std::vector<std::size_t> *invalid_index) const
02059 {
02060 static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
02061 return isPathValid(trajectory, path_constraints, emp_constraints_vector, group, verbose, invalid_index);
02062 }
02063
02064 bool planning_scene::PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory &trajectory,
02065 const std::string &group, bool verbose, std::vector<std::size_t> *invalid_index) const
02066 {
02067 static const moveit_msgs::Constraints emp_constraints;
02068 static const std::vector<moveit_msgs::Constraints> emp_constraints_vector;
02069 return isPathValid(trajectory, emp_constraints, emp_constraints_vector, group, verbose, invalid_index);
02070 }
02071
02072 void planning_scene::PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
02073 std::set<collision_detection::CostSource> &costs, double overlap_fraction) const
02074 {
02075 getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
02076 }
02077
02078
02079 void planning_scene::PlanningScene::getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs,
02080 const std::string &group_name, std::set<collision_detection::CostSource> &costs,
02081 double overlap_fraction) const
02082 {
02083 collision_detection::CollisionRequest creq;
02084 creq.max_cost_sources = max_costs;
02085 creq.group_name = group_name;
02086 creq.cost = true;
02087 std::set<collision_detection::CostSource> cs;
02088 std::set<collision_detection::CostSource> cs_start;
02089 std::size_t n_wp = trajectory.getWayPointCount();
02090 for (std::size_t i = 0 ; i < n_wp ; ++i)
02091 {
02092 collision_detection::CollisionResult cres;
02093 checkCollision(creq, cres, trajectory.getWayPoint(i));
02094 cs.insert(cres.cost_sources.begin(), cres.cost_sources.end());
02095 if (i == 0)
02096 cs_start.swap(cres.cost_sources);
02097 }
02098
02099 if (cs.size() <= max_costs)
02100 costs.swap(cs);
02101 else
02102 {
02103 costs.clear();
02104 std::size_t i = 0;
02105 for (std::set<collision_detection::CostSource>::iterator it = cs.begin() ; i < max_costs ; ++it, ++i)
02106 costs.insert(*it);
02107 }
02108
02109 collision_detection::removeCostSources(costs, cs_start, overlap_fraction);
02110 collision_detection::removeOverlapping(costs, overlap_fraction);
02111 }
02112
02113 void planning_scene::PlanningScene::getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
02114 std::set<collision_detection::CostSource> &costs) const
02115 {
02116 getCostSources(state, max_costs, std::string(), costs);
02117 }
02118
02119 void planning_scene::PlanningScene::getCostSources(const robot_state::RobotState &state, std::size_t max_costs,
02120 const std::string &group_name, std::set<collision_detection::CostSource> &costs) const
02121 {
02122 collision_detection::CollisionRequest creq;
02123 creq.max_cost_sources = max_costs;
02124 creq.group_name = group_name;
02125 creq.cost = true;
02126 collision_detection::CollisionResult cres;
02127 checkCollision(creq, cres, state);
02128 cres.cost_sources.swap(costs);
02129 }
02130
02131 void planning_scene::PlanningScene::printKnownObjects(std::ostream& out) const
02132 {
02133 const std::vector<std::string>& objects = getWorld()->getObjectIds();
02134
02135 out << "Collision World Objects:\n\t ";
02136 std::copy(objects.begin(), objects.end(), std::ostream_iterator<std::string>(out, "\n\t "));
02137
02138 std::vector<const robot_state::AttachedBody*> attached_bodies;
02139 getCurrentState().getAttachedBodies(attached_bodies);
02140
02141 out << "\nAttached Bodies:\n";
02142 for(std::size_t i = 0; i < attached_bodies.size(); ++i)
02143 {
02144 out << "\t " << attached_bodies[i]->getName() << "\n";
02145 }
02146 }