void planning_environment::convertAllowedContactSpecificationMsgToAllowedContactVector(const std::vector<arm_navigation_msgs::AllowedContactSpecification>& acmv,
std::vector<collision_space::EnvironmentModel::AllowedContact>& acv)
{
//assumes that poses are in the global frame
acv.clear();
for(unsigned int i = 0; i < acmv.size(); i++) {
const arm_navigation_msgs::AllowedContactSpecification& acs = acmv[i];
if(acs.link_names.size() != 2) {
ROS_WARN_STREAM("Allowed collision specification has link_names size " << acs.link_names.size()
<< " while the only supported size is 2");
continue;
}
shapes::Shape* shape = constructObject(acs.shape);
boost::shared_ptr<bodies::Body> bodysp(bodies::createBodyFromShape(shape));
delete shape;
tf::Transform trans;
tf::poseMsgToTF(acs.pose_stamped.pose, trans);
bodysp->setPose(trans);
collision_space::EnvironmentModel::AllowedContact allc;
allc.bound = bodysp;
allc.body_name_1 = acs.link_names[0];
allc.body_name_2 = acs.link_names[1];
allc.depth = acs.penetration_depth;
acv.push_back(allc);
}
}
void AstWhile::addNextStmt(AstNode* newp, AstNode* belowp) {
// Special, as statements need to be put in different places
// Belowp is how we came to recurse up to this point
// Preconditions insert first just before themselves (the normal rule for other statement types)
if (belowp == precondsp()) {
// Next in precond list
belowp->addNextHere(newp);
} else if (belowp == condp()) {
// Becomes first statement in body, body may have been empty
if (bodysp()) {
bodysp()->addHereThisAsNext(newp);
} else {
addBodysp(newp);
}
} else if (belowp == bodysp()) {
// Next statement in body
belowp->addNextHere(newp);
} else {
belowp->v3fatalSrc("Doesn't look like this was really under the while");
}
}
void AstWhile::addBeforeStmt(AstNode* newp, AstNode* belowp) {
// Special, as statements need to be put in different places
// Belowp is how we came to recurse up to this point
// Preconditions insert first just before themselves (the normal rule for other statement types)
if (belowp == precondsp()) {
// Must have been first statement in precondsp list, so newp is new first statement
belowp->addHereThisAsNext(newp);
} else if (belowp == condp()) {
// Goes before condition, IE in preconditions
addPrecondsp(newp);
} else if (belowp == bodysp()) {
// Was first statement in body, so new front
belowp->addHereThisAsNext(newp);
} else {
belowp->v3fatalSrc("Doesn't look like this was really under the while");
}
}
