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;
}
