planning_scene.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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   {  // the call below also calls Transforms::getTransform() too
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   /* a state is empty if it includes no information and it is a diff; if the state is not a diff, then the implicit information is
00111      that the set of attached bodies is empty, so they must be cleared from the state to be updated */
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   // Use default collision operations in the SRDF to setup the acm
00168   const std::vector<std::string>& collision_links = kmodel_->getLinkModelNamesWithCollisionGeometry();
00169   acm_->setEntry(collision_links, collision_links, false);
00170 
00171   // allow collisions for pairs that have been disabled
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 /* return NULL on failure */
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   // maintain a separate world.  Copy on write ensures that most of the object
00202   // info is shared until it is modified.
00203   world_.reset(new collision_detection::World(*parent_->world_));
00204   world_const_ = world_;
00205 
00206   // record changes to the world
00207   world_diff_.reset(new collision_detection::WorldDiff(world_));
00208 
00209   // Set up the same collision detectors as the parent
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     // leave these empty and use parent collision_robot_ unless/until a non-const one
00222     // is requested (e.g. to modify link padding or scale)
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)  // already added this one
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   // Allocate CollisionRobot unless we can use the parent's crobot_.
00306   // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_.
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   // Allocate CollisionRobot unless we can use the parent's crobot_unpadded_.
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   // clear everything, reset the world, record diffs
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   // use parent crobot_ if it exists.  Otherwise copy padding from parent.
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   // check collision with the world using the padded version
00522   getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, getAllowedCollisionMatrix());
00523 
00524   if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
00525   {
00526     // do self-collision checking with the unpadded version of the robot
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   // check collision with the world using the padded version
00545   getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobot(), kstate, acm);
00546 
00547   // do self-collision checking with the unpadded version of the robot
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   // check collision with the world using the unpadded version
00568   getCollisionWorld()->checkRobotCollision(req, res, *getCollisionRobotUnpadded(), kstate, acm);
00569 
00570   // do self-collision checking with the unpadded version of the robot
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   // add collision objects
00879   getPlanningSceneMsgCollisionObjects(scene_msg);
00880 
00881   // get the octomap
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   // add collision objects
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   // get the octomap
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   // we add object types to the planning scene, if any are specified
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   // there is at least one transform in the list of fixed transform: from model frame to itself;
01139   // if the list is empty, then nothing has been set
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   // if at least some joints have been specified, we set them
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   // if at least some links are mentioned in the allowed collision matrix, then we have an update
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   // if any colors have been specified, replace the ones we have with the specified ones
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   // process collision object updates
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   // if an octomap was specified, replace the one we have with that one
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   // each octomap replaces any previous one
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   // each octomap replaces any previous one
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       // check to see if we have the same octree pointer & pose.
01291       const shapes::OcTree *o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
01292       if (o->octree == octree)
01293       {
01294         // if the pose changed, we update it
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(); // reset this pointer first so that caching optimizations can be used in CollisionWorld
01307           world_->moveShapeInObject(OCTOMAP_NS, shape, t);
01308         }
01309         return;
01310       }
01311     }
01312   }
01313   // if the octree pointer changed, update the structure
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_) // there must be a parent in this case
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       // we need to add some shapes; if the message is empty, maybe the object is already in the world
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           // extract the shapes from the world
01375           shapes = obj->shapes_;
01376           poses = obj->shape_poses_;
01377           // remove the pointer to the objects from the collision world
01378           world_->removeObject(object.object.id);
01379 
01380           // need to transform poses to the link frame
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         // we clear the world objects with the same name, since we got an update on their geometry
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         // transform poses to link frame
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         // there should not exist an attached object with this name
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()) // if no specific object id is given, then we remove all objects attached to the link_name
01504         {
01505           kstate_->getAttachedBodies(attached_bodies, lm);
01506         }
01507         else // a specific object id will be removed
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     // replace the object if ADD is specified instead of APPEND
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     // check goal for last state
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53