diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 3ed972f36faab79319a4e359d32a6a572dee38d5..2123d20cbc9d036df5678a22e4e5592576372183 100755 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -46,14 +46,11 @@ namespace hpp { hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot) { hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot); - std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap = - problemSolver_->map - <std::vector<boost::shared_ptr<model::CollisionObject> > > (); - if (affMap.empty ()) { + if (affMap_.empty ()) { throw hpp::Error ("No affordances found. Unable to create shooter object."); } rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create - (robotcast, problemSolver_->problem ()->collisionObstacles(), affMap, + (robotcast, problemSolver_->problem ()->collisionObstacles(), affMap_, romFilter_,affFilter_,shootLimit_,displacementLimit_); if(!so3Bounds_.empty()) shooter->BoundSO3(so3Bounds_); @@ -63,7 +60,13 @@ namespace hpp { hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val) { hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot); - hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_)); + affMap_ = problemSolver_->map + <std::vector<boost::shared_ptr<model::CollisionObject> > > (); + if (affMap_.empty ()) { + throw hpp::Error ("No affordances found. Unable to create Path Validaton object."); + } + hpp::rbprm::RbPrmValidationPtr_t validation + (hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, affMap_)); hpp::core::DiscretizedCollisionCheckingPtr_t collisionChecking = hpp::core::DiscretizedCollisionChecking::create(robot,val); collisionChecking->add (validation); return collisionChecking; @@ -75,6 +78,7 @@ namespace hpp { std::size_t shootLimit_; std::size_t displacementLimit_; std::vector<double> so3Bounds_; + std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap_; }; class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder