collision_world_distance_field.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 the 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: E. Gil Jones */
00036 
00037 #include <ros/console.h>
00038 #include <moveit/collision_distance_field/collision_world_distance_field.h>
00039 #include <moveit/collision_distance_field/collision_common_distance_field.h>
00040 #include <moveit/distance_field/propagation_distance_field.h>
00041 #include <boost/make_shared.hpp>
00042 #include <boost/bind.hpp>
00043 
00044 namespace collision_detection
00045 {
00046 CollisionWorldDistanceField::~CollisionWorldDistanceField()
00047 {
00048   getWorld()->removeObserver(observer_handle_);
00049 }
00050 
00051 CollisionWorldDistanceField::CollisionWorldDistanceField(Eigen::Vector3d size, Eigen::Vector3d origin,
00052                                                          bool use_signed_distance_field, double resolution,
00053                                                          double collision_tolerance, double max_propogation_distance)
00054   : CollisionWorld()
00055   , size_(size)
00056   , origin_(origin)
00057   , use_signed_distance_field_(use_signed_distance_field)
00058   , resolution_(resolution)
00059   , collision_tolerance_(collision_tolerance)
00060   , max_propogation_distance_(max_propogation_distance)
00061 {
00062   distance_field_cache_entry_ = generateDistanceFieldCacheEntry();
00063 
00064   // request notifications about changes to world
00065   observer_handle_ =
00066       getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
00067 }
00068 
00069 CollisionWorldDistanceField::CollisionWorldDistanceField(const WorldPtr& world, Eigen::Vector3d size,
00070                                                          Eigen::Vector3d origin, bool use_signed_distance_field,
00071                                                          double resolution, double collision_tolerance,
00072                                                          double max_propogation_distance)
00073   : CollisionWorld(world)
00074   , size_(size_)
00075   , origin_(origin_)
00076   , use_signed_distance_field_(use_signed_distance_field)
00077   , resolution_(resolution)
00078   , collision_tolerance_(collision_tolerance)
00079   , max_propogation_distance_(max_propogation_distance)
00080 {
00081   distance_field_cache_entry_ = generateDistanceFieldCacheEntry();
00082 
00083   // request notifications about changes to world
00084   observer_handle_ =
00085       getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
00086   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
00087 }
00088 
00089 CollisionWorldDistanceField::CollisionWorldDistanceField(const CollisionWorldDistanceField& other,
00090                                                          const WorldPtr& world)
00091   : CollisionWorld(other, world)
00092 {
00093   size_ = other.size_;
00094   origin_ = other.origin_;
00095   use_signed_distance_field_ = other.use_signed_distance_field_;
00096   resolution_ = other.resolution_;
00097   collision_tolerance_ = other.collision_tolerance_;
00098   max_propogation_distance_ = other.max_propogation_distance_;
00099   distance_field_cache_entry_ = generateDistanceFieldCacheEntry();
00100 
00101   // request notifications about changes to world
00102   observer_handle_ =
00103       getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
00104   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
00105 }
00106 
00107 void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
00108                                                  const CollisionRobot& robot,
00109                                                  const robot_state::RobotState& state) const
00110 {
00111   boost::shared_ptr<GroupStateRepresentation> gsr;
00112   checkCollision(req, res, robot, state, gsr);
00113 }
00114 
00115 void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
00116                                                  const CollisionRobot& robot, const robot_state::RobotState& state,
00117                                                  boost::shared_ptr<GroupStateRepresentation>& gsr) const
00118 {
00119   try
00120   {
00121     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00122     if (!gsr)
00123     {
00124       cdr.generateCollisionCheckingStructures(req.group_name, state, NULL, gsr, true);
00125     }
00126     else
00127     {
00128       cdr.updateGroupStateRepresentationState(state, gsr);
00129     }
00130     bool done = cdr.getSelfCollisions(req, res, gsr);
00131     if (!done)
00132     {
00133       done = cdr.getIntraGroupCollisions(req, res, gsr);
00134     }
00135     if (!done)
00136     {
00137       getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr);
00138     }
00139   }
00140   catch (const std::bad_cast& e)
00141   {
00142     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00143     return;
00144   }
00145 
00146   (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00147 }
00148 
00149 void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
00150                                                  const CollisionRobot& robot, const robot_state::RobotState& state,
00151                                                  const AllowedCollisionMatrix& acm) const
00152 {
00153   boost::shared_ptr<GroupStateRepresentation> gsr;
00154   checkCollision(req, res, robot, state, acm, gsr);
00155 }
00156 
00157 void CollisionWorldDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
00158                                                  const CollisionRobot& robot, const robot_state::RobotState& state,
00159                                                  const AllowedCollisionMatrix& acm,
00160                                                  boost::shared_ptr<GroupStateRepresentation>& gsr) const
00161 {
00162   try
00163   {
00164     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00165     if (!gsr)
00166     {
00167       cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
00168     }
00169     else
00170     {
00171       cdr.updateGroupStateRepresentationState(state, gsr);
00172     }
00173     bool done = cdr.getSelfCollisions(req, res, gsr);
00174     if (!done)
00175     {
00176       done = cdr.getIntraGroupCollisions(req, res, gsr);
00177     }
00178     if (!done)
00179     {
00180       getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr);
00181     }
00182   }
00183   catch (const std::bad_cast& e)
00184   {
00185     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00186     return;
00187   }
00188 
00189   (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00190 }
00191 
00192 void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
00193                                                       const CollisionRobot& robot,
00194                                                       const robot_state::RobotState& state) const
00195 {
00196   boost::shared_ptr<GroupStateRepresentation> gsr;
00197   checkRobotCollision(req, res, robot, state, gsr);
00198 }
00199 
00200 void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
00201                                                       const CollisionRobot& robot, const robot_state::RobotState& state,
00202                                                       boost::shared_ptr<GroupStateRepresentation>& gsr) const
00203 {
00204   boost::shared_ptr<const distance_field::DistanceField> env_distance_field =
00205       distance_field_cache_entry_->distance_field_;
00206   try
00207   {
00208     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00209     boost::shared_ptr<const DistanceFieldCacheEntry> dfce;
00210     if (!gsr)
00211     {
00212       cdr.generateCollisionCheckingStructures(req.group_name, state, NULL, gsr, false);
00213     }
00214     else
00215     {
00216       cdr.updateGroupStateRepresentationState(state, gsr);
00217     }
00218     getEnvironmentCollisions(req, res, env_distance_field, gsr);
00219     (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00220 
00221     // checkRobotCollisionHelper(req, res, robot, state, &acm);
00222   }
00223   catch (const std::bad_cast& e)
00224   {
00225     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00226     return;
00227   }
00228 }
00229 
00230 void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
00231                                                       const CollisionRobot& robot, const robot_state::RobotState& state,
00232                                                       const AllowedCollisionMatrix& acm) const
00233 {
00234   boost::shared_ptr<GroupStateRepresentation> gsr;
00235   checkRobotCollision(req, res, robot, state, acm, gsr);
00236 }
00237 
00238 void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
00239                                                       const CollisionRobot& robot, const robot_state::RobotState& state,
00240                                                       const AllowedCollisionMatrix& acm,
00241                                                       boost::shared_ptr<GroupStateRepresentation>& gsr) const
00242 {
00243   boost::shared_ptr<const distance_field::DistanceField> env_distance_field =
00244       distance_field_cache_entry_->distance_field_;
00245   try
00246   {
00247     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00248     boost::shared_ptr<const DistanceFieldCacheEntry> dfce;
00249     if (!gsr)
00250     {
00251       cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
00252     }
00253     else
00254     {
00255       cdr.updateGroupStateRepresentationState(state, gsr);
00256     }
00257     getEnvironmentCollisions(req, res, env_distance_field, gsr);
00258     (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00259 
00260     // checkRobotCollisionHelper(req, res, robot, state, &acm);
00261   }
00262   catch (const std::bad_cast& e)
00263   {
00264     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00265     return;
00266   }
00267 }
00268 
00269 void CollisionWorldDistanceField::getCollisionGradients(const CollisionRequest& req, CollisionResult& res,
00270                                                         const CollisionRobot& robot,
00271                                                         const robot_state::RobotState& state,
00272                                                         const AllowedCollisionMatrix* acm,
00273                                                         boost::shared_ptr<GroupStateRepresentation>& gsr) const
00274 {
00275   boost::shared_ptr<const distance_field::DistanceField> env_distance_field =
00276       distance_field_cache_entry_->distance_field_;
00277   try
00278   {
00279     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00280     if (!gsr)
00281     {
00282       cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
00283     }
00284     else
00285     {
00286       cdr.updateGroupStateRepresentationState(state, gsr);
00287     }
00288     cdr.getSelfProximityGradients(gsr);
00289     cdr.getIntraGroupProximityGradients(gsr);
00290     getEnvironmentProximityGradients(env_distance_field, gsr);
00291   }
00292   catch (const std::bad_cast& e)
00293   {
00294     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00295     return;
00296   }
00297 
00298   (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00299 }
00300 
00301 void CollisionWorldDistanceField::getAllCollisions(const CollisionRequest& req, CollisionResult& res,
00302                                                    const CollisionRobot& robot, const robot_state::RobotState& state,
00303                                                    const AllowedCollisionMatrix* acm,
00304                                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const
00305 {
00306   try
00307   {
00308     const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
00309     if (!gsr)
00310     {
00311       cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
00312     }
00313     else
00314     {
00315       cdr.updateGroupStateRepresentationState(state, gsr);
00316     }
00317     cdr.getSelfCollisions(req, res, gsr);
00318     cdr.getIntraGroupCollisions(req, res, gsr);
00319     boost::shared_ptr<const distance_field::DistanceField> env_distance_field =
00320         distance_field_cache_entry_->distance_field_;
00321     getEnvironmentCollisions(req, res, env_distance_field, gsr);
00322   }
00323   catch (const std::bad_cast& e)
00324   {
00325     ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
00326     return;
00327   }
00328 
00329   (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
00330 }
00331 
00332 bool CollisionWorldDistanceField::getEnvironmentCollisions(
00333     const CollisionRequest& req, CollisionResult& res,
00334     const boost::shared_ptr<const distance_field::DistanceField>& env_distance_field,
00335     boost::shared_ptr<GroupStateRepresentation>& gsr) const
00336 {
00337   for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++)
00338   {
00339     bool is_link = i < gsr->dfce_->link_names_.size();
00340     std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached";
00341     if (is_link && !gsr->dfce_->link_has_geometry_[i])
00342     {
00343       continue;
00344     }
00345 
00346     const std::vector<CollisionSphere>* collision_spheres_1;
00347     const EigenSTL::vector_Vector3d* sphere_centers_1;
00348 
00349     if (is_link)
00350     {
00351       collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
00352       sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
00353     }
00354     else
00355     {
00356       collision_spheres_1 =
00357           &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
00358       sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
00359     }
00360 
00361     if (req.contacts)
00362     {
00363       std::vector<unsigned int> colls;
00364       bool coll = getCollisionSphereCollision(
00365           env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_,
00366           collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
00367       if (coll)
00368       {
00369         res.collision = true;
00370         for (unsigned int j = 0; j < colls.size(); j++)
00371         {
00372           Contact con;
00373           if (is_link)
00374           {
00375             con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[colls[j]];
00376             con.body_type_1 = BodyTypes::ROBOT_LINK;
00377             con.body_name_1 = gsr->dfce_->link_names_[i];
00378           }
00379           else
00380           {
00381             con.pos =
00382                 gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[colls[j]];
00383             con.body_type_1 = BodyTypes::ROBOT_ATTACHED;
00384             con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
00385           }
00386 
00387           con.body_type_2 = BodyTypes::WORLD_OBJECT;
00388           con.body_name_2 = "environment";
00389           res.contact_count++;
00390           res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
00391           gsr->gradients_[i].types[colls[j]] = ENVIRONMENT;
00392           // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " <<
00393           // colls[j] << " in env collision");
00394         }
00395 
00396         gsr->gradients_[i].collision = true;
00397         if (res.contact_count >= req.max_contacts)
00398         {
00399           return true;
00400         }
00401       }
00402     }
00403     else
00404     {
00405       bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
00406                                               max_propogation_distance_, collision_tolerance_);
00407       if (coll)
00408       {
00409         res.collision = true;
00410         return true;
00411       }
00412     }
00413   }
00414   return (res.contact_count >= req.max_contacts);
00415 }
00416 
00417 bool CollisionWorldDistanceField::getEnvironmentProximityGradients(
00418     const boost::shared_ptr<const distance_field::DistanceField>& env_distance_field,
00419     boost::shared_ptr<GroupStateRepresentation>& gsr) const
00420 {
00421   bool in_collision = false;
00422   for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
00423   {
00424     bool is_link = i < gsr->dfce_->link_names_.size();
00425 
00426     if (is_link && !gsr->dfce_->link_has_geometry_[i])
00427     {
00428       continue;
00429     }
00430 
00431     const std::vector<CollisionSphere>* collision_spheres_1;
00432     const EigenSTL::vector_Vector3d* sphere_centers_1;
00433     if (is_link)
00434     {
00435       collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
00436       sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
00437     }
00438     else
00439     {
00440       collision_spheres_1 =
00441           &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
00442       sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
00443     }
00444 
00445     bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
00446                                             gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false,
00447                                             max_propogation_distance_, false);
00448     if (coll)
00449     {
00450       in_collision = true;
00451     }
00452   }
00453   return in_collision;
00454 }
00455 
00456 void CollisionWorldDistanceField::setWorld(const WorldPtr& world)
00457 {
00458   if (world == getWorld())
00459     return;
00460 
00461   // turn off notifications about old world
00462   getWorld()->removeObserver(observer_handle_);
00463 
00464   // clear out objects from old world
00465   distance_field_cache_entry_->distance_field_->reset();
00466 
00467   CollisionWorld::setWorld(world);
00468 
00469   // request notifications about changes to new world
00470   observer_handle_ =
00471       getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
00472 
00473   // get notifications any objects already in the new world
00474   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
00475 }
00476 
00477 void CollisionWorldDistanceField::notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj,
00478                                                      World::Action action)
00479 {
00480   ros::WallTime n = ros::WallTime::now();
00481 
00482   EigenSTL::vector_Vector3d add_points;
00483   EigenSTL::vector_Vector3d subtract_points;
00484   self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_, add_points, subtract_points);
00485 
00486   if (action == World::DESTROY)
00487   {
00488     self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
00489   }
00490   else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE))
00491   {
00492     self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
00493     self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points);
00494   }
00495   else
00496   {
00497     self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points);
00498   }
00499 
00500   logDebug("Modifying object %s took %lf s", obj->id_.c_str(), (ros::WallTime::now() - n).toSec());
00501 }
00502 
00503 void CollisionWorldDistanceField::updateDistanceObject(
00504     const std::string& id, boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>& dfce,
00505     EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points)
00506 {
00507   std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>>::iterator cur_it =
00508       dfce->posed_body_point_decompositions_.find(id);
00509   if (cur_it != dfce->posed_body_point_decompositions_.end())
00510   {
00511     for (unsigned int i = 0; i < cur_it->second.size(); i++)
00512     {
00513       subtract_points.insert(subtract_points.end(), cur_it->second[i]->getCollisionPoints().begin(),
00514                              cur_it->second[i]->getCollisionPoints().end());
00515     }
00516   }
00517 
00518   World::ObjectConstPtr object = getWorld()->getObject(id);
00519   if (object)
00520   {
00521     ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size()
00522                                                 << " shapes  to CollisionWorldDistanceField");
00523     std::vector<PosedBodyPointDecompositionPtr> shape_points;
00524     for (unsigned int i = 0; i < object->shapes_.size(); i++)
00525     {
00526       shapes::ShapeConstPtr shape = object->shapes_[i];
00527       if (shape->type == shapes::OCTREE)
00528       {
00529         const shapes::OcTree* octree_shape = static_cast<const shapes::OcTree*>(shape.get());
00530         boost::shared_ptr<const octomap::OcTree> octree = octree_shape->octree;
00531 
00532         shape_points.push_back(boost::make_shared<PosedBodyPointDecomposition>(octree));
00533       }
00534       else
00535       {
00536         BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_);
00537 
00538         shape_points.push_back(boost::make_shared<PosedBodyPointDecomposition>(bd, object->shape_poses_[i]));
00539       }
00540 
00541       add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(),
00542                         shape_points.back()->getCollisionPoints().end());
00543     }
00544 
00545     dfce->posed_body_point_decompositions_[id] = shape_points;
00546   }
00547   else
00548   {
00549     ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionWorldDistanceField");
00550     dfce->posed_body_point_decompositions_.erase(id);
00551   }
00552 }
00553 
00554 boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>
00555 CollisionWorldDistanceField::generateDistanceFieldCacheEntry()
00556 {
00557   boost::shared_ptr<DistanceFieldCacheEntry> dfce(new DistanceFieldCacheEntry());
00558   dfce->distance_field_.reset(new distance_field::PropagationDistanceField(
00559       size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
00560       origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_));
00561 
00562   EigenSTL::vector_Vector3d add_points;
00563   EigenSTL::vector_Vector3d subtract_points;
00564   for (World::const_iterator it = getWorld()->begin(); it != getWorld()->end(); ++it)
00565   {
00566     updateDistanceObject(it->first, dfce, add_points, subtract_points);
00567   }
00568   dfce->distance_field_->addPointsToField(add_points);
00569   return dfce;
00570 }
00571 }
00572 
00573 #include <moveit/collision_distance_field/collision_detector_allocator_distance_field.h>
00574 const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME_("DISTANCE_FIELD");


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Mon Jul 24 2017 02:21:02