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