void collision_detection::CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const planning_models::KinematicState &state,
const AllowedCollisionMatrix *acm) const
{
FCLManager manager;
allocSelfCollisionBroadPhase(state, manager);
CollisionData cd(&req, &res, acm);
cd.enableGroup(getKinematicModel());
manager.manager_->collide(&cd, &collisionCallback);
if (req.distance)
res.distance = distanceSelfHelper(state, acm);
}
double collision_detection::CollisionRobotFCL::distanceSelfHelper(const planning_models::KinematicState &state,
const AllowedCollisionMatrix *acm) const
{
FCLManager manager;
allocSelfCollisionBroadPhase(state, manager);
CollisionRequest req;
CollisionResult res;
CollisionData cd(&req, &res, acm);
cd.enableGroup(getKinematicModel());
manager.manager_->distance(&cd, &distanceCallback);
return res.distance;
}
void collision_detection::CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest &req, CollisionResult &res, const planning_models::KinematicState &state,
const CollisionRobot &other_robot, const planning_models::KinematicState &other_state,
const AllowedCollisionMatrix *acm) const
{
FCLManager manager;
allocSelfCollisionBroadPhase(state, manager);
const CollisionRobotFCL &fcl_rob = dynamic_cast<const CollisionRobotFCL&>(other_robot);
FCLObject other_fcl_obj;
fcl_rob.constructFCLObject(other_state, other_fcl_obj);
CollisionData cd(&req, &res, acm);
cd.enableGroup(getKinematicModel());
for (std::size_t i = 0 ; !cd.done_ && i < other_fcl_obj.collision_objects_.size() ; ++i)
manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
if (req.distance)
res.distance = distanceOtherHelper(state, other_robot, other_state, acm);
}
bool planning_environment::PlanningMonitor::getCompletePlanningScene(const arm_navigation_msgs::PlanningScene& planning_diff,
const arm_navigation_msgs::OrderedCollisionOperations& ordered_collision_operations,
arm_navigation_msgs::PlanningScene& planning_scene) const{
{
//indenting because we only need the state in here
//creating state
planning_models::KinematicState set_state(getKinematicModel());
//setting state to current values
setStateValuesFromCurrentValues(set_state);
//supplementing with state_diff
setRobotStateAndComputeTransforms(planning_diff.robot_state, set_state);
//now complete robot state is populated
convertKinematicStateToRobotState(set_state,
last_joint_state_update_,
cm_->getWorldFrameId(),
planning_scene.robot_state);
}
//getting full list of tf transforms not associated with the robot's body
getAllFixedFrameTransforms(planning_scene.fixed_frame_transforms);
//getting all the stuff from the current collision space
collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCollisionSpace()->getDefaultAllowedCollisionMatrix();
if(planning_diff.allowed_collision_matrix.link_names.size() > 0) {
acm = convertFromACMMsgToACM(planning_diff.allowed_collision_matrix);
}
//first we deal with collision object diffs
cm_->getCollisionSpaceCollisionObjects(planning_scene.collision_objects);
for(unsigned int i = 0; i < planning_scene.collision_objects.size(); i++) {
if(!acm.hasEntry(planning_scene.collision_objects[i].id)) {
ROS_ERROR_STREAM("Sanity check failing - no entry in acm for collision space object " << planning_scene.collision_objects[i].id);
}
}
cm_->getLastCollisionMap(planning_scene.collision_map);
//probably haven't gotten another collision map yet after a clear
if(planning_scene.collision_map.boxes.size() > 0 && !acm.hasEntry(COLLISION_MAP_NAME)) {
ROS_INFO_STREAM("Adding entry for collision map");
acm.addEntry(COLLISION_MAP_NAME, false);
}
//now attached objects
cm_->getCollisionSpaceAttachedCollisionObjects(planning_scene.attached_collision_objects);
for(unsigned int i = 0; i < planning_scene.attached_collision_objects.size(); i++) {
if(!acm.hasEntry(planning_scene.attached_collision_objects[i].object.id)) {
ROS_ERROR_STREAM("Sanity check failing - no entry in acm for attached collision space object " << planning_scene.attached_collision_objects[i].object.id);
}
}
std::map<std::string, double> cur_link_padding = cm_->getCollisionSpace()->getCurrentLinkPaddingMap();
for(unsigned int i = 0; i < planning_diff.collision_objects.size(); i++) {
std::string object_name = planning_diff.collision_objects[i].id;
if(object_name == "all") {
if(planning_diff.collision_objects[i].operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
for(unsigned int j = 0; j < planning_scene.collision_objects.size(); i++) {
acm.removeEntry(planning_scene.collision_objects[j].id);
}
planning_scene.collision_objects.clear();
continue;
} else {
ROS_WARN_STREAM("No other operation than remove permitted for all");
return false;
}
}
bool already_have = false;
std::vector<arm_navigation_msgs::CollisionObject>::iterator it = planning_scene.collision_objects.begin();
while(it != planning_scene.collision_objects.end()) {
if((*it).id == object_name) {
already_have = true;
break;
}
it++;
}
if(planning_diff.collision_objects[i].operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
if(!already_have) {
ROS_WARN_STREAM("Diff remove specified for object " << object_name << " which we don't seem to have");
continue;
}
acm.removeEntry(object_name);
planning_scene.collision_objects.erase(it);
} else {
//must be an add
if(already_have) {
//if we already have it don't need to add to the matrix
planning_scene.collision_objects.erase(it);
} else {
acm.addEntry(object_name, false);
}
planning_scene.collision_objects.push_back(planning_diff.collision_objects[i]);
}
}
for(unsigned int i = 0; i < planning_diff.attached_collision_objects.size(); i++) {
std::string link_name = planning_diff.attached_collision_objects[i].link_name;
std::string object_name = planning_diff.attached_collision_objects[i].object.id;
if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT ||
planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT) {
ROS_WARN_STREAM("Object replacement not supported during diff");
}
if(link_name == "all") {
if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
for(unsigned int j = 0; j < planning_scene.attached_collision_objects.size(); i++) {
acm.removeEntry(planning_scene.attached_collision_objects[j].object.id);
}
planning_scene.attached_collision_objects.clear();
continue;
} else {
ROS_WARN_STREAM("No other operation than remove permitted for all");
return false;
}
} else {
if(object_name == "all") {
if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
std::vector<arm_navigation_msgs::AttachedCollisionObject>::iterator it = planning_scene.attached_collision_objects.begin();
while(it != planning_scene.attached_collision_objects.end()) {
if((*it).link_name == link_name) {
acm.removeEntry((*it).object.id);
it == planning_scene.attached_collision_objects.erase(it);
} else {
it++;
}
}
} else {
ROS_WARN_STREAM("No other operation than remove permitted for all");
return false;
}
continue;
}
bool already_have = false;
std::vector<arm_navigation_msgs::AttachedCollisionObject>::iterator it = planning_scene.attached_collision_objects.begin();
while(it != planning_scene.attached_collision_objects.end()) {
if((*it).link_name == link_name) {
if((*it).object.id == object_name) {
already_have = true;
break;
}
}
it++;
}
if(planning_diff.attached_collision_objects[i].object.operation.operation == arm_navigation_msgs::CollisionObjectOperation::REMOVE) {
if(!already_have) {
ROS_WARN_STREAM("Diff remove specified for object " << object_name << " which we don't seem to have");
return false;
}
acm.removeEntry((*it).object.id);
planning_scene.attached_collision_objects.erase(it);
} else {
//must be an add
if(already_have) {
planning_scene.attached_collision_objects.erase(it);
} else {
acm.addEntry(planning_diff.attached_collision_objects[i].object.id, false);
}
acm.changeEntry(planning_diff.attached_collision_objects[i].object.id, planning_diff.attached_collision_objects[i].link_name, true);
std::vector<std::string> modded_touch_links;
for(unsigned int j = 0; j < planning_diff.attached_collision_objects[i].touch_links.size(); j++) {
if(cm_->getKinematicModel()->getModelGroup(planning_diff.attached_collision_objects[i].touch_links[j])) {
std::vector<std::string> links = cm_->getKinematicModel()->getModelGroup(planning_diff.attached_collision_objects[i].touch_links[j])->getGroupLinkNames();
modded_touch_links.insert(modded_touch_links.end(), links.begin(), links.end());
} else {
modded_touch_links.push_back(planning_diff.attached_collision_objects[i].touch_links[j]);
}
}
for(unsigned int j = 0; j < modded_touch_links.size(); j++) {
acm.changeEntry(planning_diff.attached_collision_objects[i].object.id, modded_touch_links[j], true);
}
planning_scene.attached_collision_objects.push_back(planning_diff.attached_collision_objects[i]);
}
}
}
for(unsigned int i = 0; i < planning_diff.link_padding.size(); i++) {
//note - for things like attached objects this might just add them to the mix, but that's fine
cur_link_padding[planning_diff.link_padding[i].link_name] = planning_diff.link_padding[i].padding;
}
convertFromLinkPaddingMapToLinkPaddingVector(cur_link_padding, planning_scene.link_padding);
//now we need to apply the allowed collision operations to the modified ACM
std::vector<std::string> o_strings;
for(unsigned int i = 0; i < planning_scene.collision_objects.size(); i++) {
o_strings.push_back(planning_scene.collision_objects[i].id);
}
std::vector<std::string> a_strings;
for(unsigned int i = 0; i < planning_scene.attached_collision_objects.size(); i++) {
a_strings.push_back(planning_scene.attached_collision_objects[i].object.id);
}
applyOrderedCollisionOperationsListToACM(ordered_collision_operations,
o_strings,
a_strings,
cm_->getKinematicModel(),
acm);
convertFromACMToACMMsg(acm, planning_scene.allowed_collision_matrix);
planning_scene.allowed_contacts = planning_diff.allowed_contacts;
return true;
}
