def get_objects_intersect_objects( req ): then = rospy.Time.now() rospy.logdebug( "SEMAP DB SRVs: get_objects_intersect_objects" ) res = GetObjectsIntersectObjectsResponse() if req.reference.geometry_type in [ "AxisAligned3D", "BoundingBox", "Body" ]: #"Position3D", "BoundingHull" ref_status = "3D" elif req.reference.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]: ref_status = "2D" else: rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported", req.reference.geometry_type) return res if req.target.geometry_type in [ "AxisAligned3D", "BoundingBox", "Body" ]: #"Position3D", "BoundingHull" tar_status = "3D" elif req.target.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]: tar_status = "2D" else: rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported", req.target.geometry_type) return res if ref_status != tar_status: rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with requests of inconsistent dimensionality") return res ref_obj = aliased( ObjectInstance ) ref_geo = aliased( GeometryModel ) tar_obj = aliased( ObjectInstance ) tar_geo = aliased( GeometryModel ) #if req.reference.ids and req.reference.ids != (0,): # ref_ids = req.reference.ids #elif req.reference.class_types: # ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all() #else: # ref_ids = any_obj_ids( ref_obj ).all() #if req.target.ids and req.target.ids != (0,): # tar_ids = req.target.ids #elif req.target.class_types: # tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all() #else: # tar_ids = any_obj_ids( tar_obj ).all() ref_ids =constrain_obj_ids(ref_obj, req.reference).all() tar_ids =constrain_obj_ids(tar_obj, req.target).all() if ref_status == "2D": operator = ST_Overlaps(tar_geo.geometry, ref_geo.geometry) else: operator = SFCGAL_Intersects3D(ref_geo.geometry, tar_geo.geometry) retrieval = ( rospy.Time.now() - then ).to_sec() if req.reference.geometry_type == "Body" and req.target.geometry_type == "Body": ids = db().query( ref_obj.id, tar_obj.id ).\ filter( ref_obj.id.in_( ref_ids ), ref_obj.absolute_description_id == ref_geo.geometry_desc, ref_geo.type == req.reference.geometry_type, tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.geometry_desc, tar_geo.type == req.target.geometry_type, ref_obj.id != tar_obj.id, operator).order_by(ref_obj.id).all() else: #print 'ref sql', constrain_obj_ids(ref_obj, req.reference) #print 'ref geo', constrain_obj_ids(ref_obj, req.reference).all() res.refs = len(ref_ids) res.tars = len(tar_ids) res.num_tests = len(ref_ids) * len(tar_ids) #test = db().query( ref_obj.id, tar_obj.id ).\ #filter( #ref_geo.id.in_( constrain_geo_ids(ref_obj, ref_geo, req.reference).correlate(ref_obj.absolute_description_id == ref_geo.geometry_desc) ), #tar_geo.id.in_( constrain_geo_ids(tar_obj, tar_geo, req.target).correlate(tar_obj.absolute_description_id == tar_geo.geometry_desc) ), #ref_obj.id != tar_obj.id, #operator #).all() #print 'ref', constrain_obj_ids(ref_obj, req.reference).all() #print 'tar', constrain_obj_ids(tar_obj, req.target).all() #print test ids = db().query( ref_obj.id, tar_obj.id ).\ filter( #or_( ref_obj.id.in_( ref_ids ), ref_obj.id.in_( req.reference.ids )) ref_obj.id.in_( constrain_obj_ids(ref_obj, req.reference) ), ref_obj.absolute_description_id == ref_geo.abstraction_desc, ref_geo.type == req.reference.geometry_type, tar_obj.id.in_( constrain_obj_ids(tar_obj, req.target) ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type, ref_obj.id != tar_obj.id, operator).order_by(ref_obj.id).all() testing = ( rospy.Time.now() - then ).to_sec() - retrieval if ids: old_i = None one_to_many = OneToMany() one_to_many.reference_id = ids[0][0] for i, d in ids: if one_to_many.reference_id != i: res.results.append(one_to_many) one_to_many = OneToMany() one_to_many.reference_id = i one_to_many.target_ids.append(d) pair = ObjectPair() pair.reference_id = i pair.target_id = d pair.relations.append("intersects") res.pairs.append(pair) res.results.append(one_to_many) packaging = ( rospy.Time.now() - then ).to_sec() - retrieval - testing #rospy.loginfo( "ObjectsIntersectObjects %f:" % ( rospy.Time.now() - then ).to_sec() ) #rospy.loginfo( " Retrieving %f:" % retrieval) #rospy.loginfo( " Testing %f:" % testing) #rospy.loginfo( " Packaging %f:" % packaging ) #print '#pairs', len(res.pairs) return res
def get_objects_intersect_objects(req): then = rospy.Time.now() rospy.logdebug("SEMAP DB SRVs: get_objects_intersect_objects") res = GetObjectsIntersectObjectsResponse() if req.reference.geometry_type in ["AxisAligned3D", "BoundingBox", "Body"]: #"Position3D", "BoundingHull" ref_status = "3D" elif req.reference.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]: ref_status = "2D" else: rospy.logerr( "SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported", req.reference.geometry_type) return res if req.target.geometry_type in ["AxisAligned3D", "BoundingBox", "Body"]: #"Position3D", "BoundingHull" tar_status = "3D" elif req.target.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]: tar_status = "2D" else: rospy.logerr( "SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported", req.target.geometry_type) return res if ref_status != tar_status: rospy.logerr( "SEMAP DB SRVs: get_objects_in_objects was called with requests of inconsistent dimensionality" ) return res ref_obj = aliased(ObjectInstance) ref_geo = aliased(GeometryModel) tar_obj = aliased(ObjectInstance) tar_geo = aliased(GeometryModel) #if req.reference.ids and req.reference.ids != (0,): # ref_ids = req.reference.ids #elif req.reference.class_types: # ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all() #else: # ref_ids = any_obj_ids( ref_obj ).all() #if req.target.ids and req.target.ids != (0,): # tar_ids = req.target.ids #elif req.target.class_types: # tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all() #else: # tar_ids = any_obj_ids( tar_obj ).all() ref_ids = constrain_obj_ids(ref_obj, req.reference).all() tar_ids = constrain_obj_ids(tar_obj, req.target).all() if ref_status == "2D": operator = ST_Overlaps(tar_geo.geometry, ref_geo.geometry) else: operator = SFCGAL_Intersects3D(ref_geo.geometry, tar_geo.geometry) retrieval = (rospy.Time.now() - then).to_sec() if req.reference.geometry_type == "Body" and req.target.geometry_type == "Body": ids = db().query( ref_obj.id, tar_obj.id ).\ filter( ref_obj.id.in_( ref_ids ), ref_obj.absolute_description_id == ref_geo.geometry_desc, ref_geo.type == req.reference.geometry_type, tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.geometry_desc, tar_geo.type == req.target.geometry_type, ref_obj.id != tar_obj.id, operator).order_by(ref_obj.id).all() else: #print 'ref sql', constrain_obj_ids(ref_obj, req.reference) #print 'ref geo', constrain_obj_ids(ref_obj, req.reference).all() res.refs = len(ref_ids) res.tars = len(tar_ids) res.num_tests = len(ref_ids) * len(tar_ids) #test = db().query( ref_obj.id, tar_obj.id ).\ #filter( #ref_geo.id.in_( constrain_geo_ids(ref_obj, ref_geo, req.reference).correlate(ref_obj.absolute_description_id == ref_geo.geometry_desc) ), #tar_geo.id.in_( constrain_geo_ids(tar_obj, tar_geo, req.target).correlate(tar_obj.absolute_description_id == tar_geo.geometry_desc) ), #ref_obj.id != tar_obj.id, #operator #).all() #print 'ref', constrain_obj_ids(ref_obj, req.reference).all() #print 'tar', constrain_obj_ids(tar_obj, req.target).all() #print test ids = db().query( ref_obj.id, tar_obj.id ).\ filter( #or_( ref_obj.id.in_( ref_ids ), ref_obj.id.in_( req.reference.ids )) ref_obj.id.in_( constrain_obj_ids(ref_obj, req.reference) ), ref_obj.absolute_description_id == ref_geo.abstraction_desc, ref_geo.type == req.reference.geometry_type, tar_obj.id.in_( constrain_obj_ids(tar_obj, req.target) ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type, ref_obj.id != tar_obj.id, operator).order_by(ref_obj.id).all() testing = (rospy.Time.now() - then).to_sec() - retrieval if ids: old_i = None one_to_many = OneToMany() one_to_many.reference_id = ids[0][0] for i, d in ids: if one_to_many.reference_id != i: res.results.append(one_to_many) one_to_many = OneToMany() one_to_many.reference_id = i one_to_many.target_ids.append(d) pair = ObjectPair() pair.reference_id = i pair.target_id = d pair.relations.append("intersects") res.pairs.append(pair) res.results.append(one_to_many) packaging = (rospy.Time.now() - then).to_sec() - retrieval - testing #rospy.loginfo( "ObjectsIntersectObjects %f:" % ( rospy.Time.now() - then ).to_sec() ) #rospy.loginfo( " Retrieving %f:" % retrieval) #rospy.loginfo( " Testing %f:" % testing) #rospy.loginfo( " Packaging %f:" % packaging ) #print '#pairs', len(res.pairs) return res
def get_objects_on_objects( req ): rospy.logdebug( "SEMAP DB SRVs: get_objects_on_objects" ) res = GetObjectsOnObjectsResponse() if req.reference.geometry_type not in [ "BoundingBox" ]: #"Position3D", "BoundingHull" rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported", req.reference.geometry_type) return res if req.target.geometry_type not in [ "BoundingBox" ]: #"Position3D", "BoundingHull" rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported", req.reference.geometry_type) return res ref_obj = aliased( ObjectInstance ) ref_geo = aliased( GeometryModel ) ref_top = aliased( GeometryModel ) tar_obj = aliased( ObjectInstance ) tar_geo = aliased( GeometryModel ) #if req.reference.ids and req.reference.ids != (0,): #ref_ids = req.reference.ids #elif req.reference.class_types: #ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all() #else: #ref_ids = any_obj_ids( ref_obj ).all() #if req.target.ids and req.target.ids != (0,): #tar_ids = req.target.ids #elif req.target.class_types: #tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all() #else: #tar_ids = any_obj_ids( tar_obj ).all() ref_ids =constrain_obj_ids(ref_obj, req.reference).all() tar_ids =constrain_obj_ids(tar_obj, req.target).all() res.refs = len(ref_ids) res.tars = len(tar_ids) res.num_tests = len(ref_ids) * len(tar_ids) print 'ON TESTS', res.num_tests print 'refs', ref_ids print 'refs', tar_ids pairs = db().query( ref_obj.id, tar_obj.id ).\ filter( ref_obj.id.in_( ref_ids ), ref_obj.absolute_description_id == ref_geo.abstraction_desc, ref_geo.type == req.reference.geometry_type, ref_obj.absolute_description_id == ref_top.abstraction_desc, ref_top.type == "TopProjection", tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type, SFCGAL_Contains3D( ref_top.geometry, tar_geo.geometry), SFCGAL_Distance3D( ref_geo.geometry, tar_geo.geometry) < req.threshold).order_by(ref_obj.id).all() if pairs: old_ref = None one_to_many = OneToMany() one_to_many.reference_id = pairs[0][0] for ref_id, tar_id in pairs: if one_to_many.reference_id != ref_id: res.results.append(one_to_many) one_to_many = OneToMany() one_to_many.reference_id = ref_id one_to_many.target_ids.append(tar_id) pair = ObjectPair() pair.reference_id = ref_id pair.target_id = tar_id pair.relations.append("contains") res.pairs.append(pair) res.results.append(one_to_many) return res
def get_objects_on_objects(req): rospy.logdebug("SEMAP DB SRVs: get_objects_on_objects") res = GetObjectsOnObjectsResponse() if req.reference.geometry_type not in ["BoundingBox" ]: #"Position3D", "BoundingHull" rospy.logerr( "SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported", req.reference.geometry_type) return res if req.target.geometry_type not in ["BoundingBox" ]: #"Position3D", "BoundingHull" rospy.logerr( "SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported", req.reference.geometry_type) return res ref_obj = aliased(ObjectInstance) ref_geo = aliased(GeometryModel) ref_top = aliased(GeometryModel) tar_obj = aliased(ObjectInstance) tar_geo = aliased(GeometryModel) #if req.reference.ids and req.reference.ids != (0,): #ref_ids = req.reference.ids #elif req.reference.class_types: #ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all() #else: #ref_ids = any_obj_ids( ref_obj ).all() #if req.target.ids and req.target.ids != (0,): #tar_ids = req.target.ids #elif req.target.class_types: #tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all() #else: #tar_ids = any_obj_ids( tar_obj ).all() ref_ids = constrain_obj_ids(ref_obj, req.reference).all() tar_ids = constrain_obj_ids(tar_obj, req.target).all() res.refs = len(ref_ids) res.tars = len(tar_ids) res.num_tests = len(ref_ids) * len(tar_ids) print 'ON TESTS', res.num_tests print 'refs', ref_ids print 'refs', tar_ids pairs = db().query( ref_obj.id, tar_obj.id ).\ filter( ref_obj.id.in_( ref_ids ), ref_obj.absolute_description_id == ref_geo.abstraction_desc, ref_geo.type == req.reference.geometry_type, ref_obj.absolute_description_id == ref_top.abstraction_desc, ref_top.type == "TopProjection", tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type, SFCGAL_Contains3D( ref_top.geometry, tar_geo.geometry), SFCGAL_Distance3D( ref_geo.geometry, tar_geo.geometry) < req.threshold).order_by(ref_obj.id).all() if pairs: old_ref = None one_to_many = OneToMany() one_to_many.reference_id = pairs[0][0] for ref_id, tar_id in pairs: if one_to_many.reference_id != ref_id: res.results.append(one_to_many) one_to_many = OneToMany() one_to_many.reference_id = ref_id one_to_many.target_ids.append(tar_id) pair = ObjectPair() pair.reference_id = ref_id pair.target_id = tar_id pair.relations.append("contains") res.pairs.append(pair) res.results.append(one_to_many) return res