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