void CollisionWorldDistanceField::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const planning_models::KinematicState &state, boost::shared_ptr<GroupStateRepresentation>& gsr) const { try { const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot); if(!gsr) { cdr.generateCollisionCheckingStructures(req.group_name, state, NULL, gsr, true); } else { cdr.updateGroupStateRepresentationState(state, gsr); } bool done = cdr.getSelfCollisions(req, res, gsr); if(!done) { done = cdr.getIntraGroupCollisions(req, res, gsr); } if(!done) { getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); } } catch(...) { ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField"); return; } //(const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr; }
void CollisionWorldDistanceField::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const planning_models::KinematicState &state, const AllowedCollisionMatrix &acm, boost::shared_ptr<GroupStateRepresentation>& gsr) const { boost::shared_ptr<const distance_field::DistanceField> env_distance_field = distance_field_cache_entry_->distance_field_; try { const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot); boost::shared_ptr<const DistanceFieldCacheEntry> dfce; if(!gsr) { cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, false); } else { cdr.updateGroupStateRepresentationState(state, gsr); } getEnvironmentCollisions(req, res, env_distance_field, gsr); //(const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr; //checkRobotCollisionHelper(req, res, robot, state, &acm); } catch(...) { ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField"); return; } }
void CollisionWorldDistanceField::getAllCollisions(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const planning_models::KinematicState &state, const AllowedCollisionMatrix* acm, boost::shared_ptr<GroupStateRepresentation>& gsr) const { try { const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot); if(!gsr) { cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true); } else { cdr.updateGroupStateRepresentationState(state, gsr); } cdr.getSelfCollisions(req, res, gsr); cdr.getIntraGroupCollisions(req, res, gsr); boost::shared_ptr<const distance_field::DistanceField> env_distance_field = distance_field_cache_entry_->distance_field_; getEnvironmentCollisions(req, res, env_distance_field, gsr); } catch(...) { ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField"); return; } }
void CollisionWorldDistanceField::checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm, boost::shared_ptr<GroupStateRepresentation>& gsr) const { try { const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot); if(!gsr) { cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true); } else { cdr.updateGroupStateRepresentationState(state, gsr); } bool done = cdr.getSelfCollisions(req, res, gsr); if(!done) { done = cdr.getIntraGroupCollisions(req, res, gsr); } if(!done) { getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr); } } catch(...) { logError("Could not cast CollisionRobot to CollisionRobotDistanceField"); return; } //(const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr; }