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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44