def get_containment_pairs( req ): rospy.loginfo( "SEMAP DB SRVs: get_containment_pairs" ) res = GetObjectLocation() obj1 = aliased( ObjectInstance ) geo1 = aliased( GeometryModel ) obj2 = aliased( ObjectInstance ) geo2 = aliased( GeometryModel ) if req.reference_object_types: obj1_ids = any_obj_types_ids(obj1, req.refrence_object_types) else: obj1_ids = any_obj_ids(obj1) if req.target_object_types: obj2_ids = any_obj_types_ids(obj2, req.target_object_types) else: obj2_ids = any_obj_ids(obj2) ids = db().query( obj1.id, obj2.id ).\ filter(obj1.id.in_( obj1_ids ), obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \ obj2.id.in_( obj2_ids ), obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox", \ SFCGAL_Contains3D( geo1.geometry, geo2.geometry) ).all() for i1, i2 in ids: pair = ObjectPair() pair.reference_id = i1 pair.relations.append("contains") pair.target_id = i2 res.target_object_ids = [id for id, in ids] return res
def get_objects_within_volume( req ): rospy.loginfo( "SEMAP DB SRVs: get_objects_within_area" ) res = GetObjectsWithinVolumeResponse() tar_obj = aliased( ObjectInstance ) tar_geo = aliased( GeometryModel ) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.fully_within: operator = SFCGAL_Contains3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry) else: operator = or_( SFCGAL_Contains3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry), SFCGAL_Intersects3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry) ) results = db().query( tar_obj.id ).\ filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).all() for i in results: pair = ObjectPair() pair.reference_id = -1 pair.target_id = i[0] pair.relations.append("contained-in-volume") res.pairs.append(pair) return res '''
def get_containment_pairs(req): rospy.loginfo("SEMAP DB SRVs: get_containment_pairs") res = GetObjectLocation() obj1 = aliased(ObjectInstance) geo1 = aliased(GeometryModel) obj2 = aliased(ObjectInstance) geo2 = aliased(GeometryModel) if req.reference_object_types: obj1_ids = any_obj_types_ids(obj1, req.refrence_object_types) else: obj1_ids = any_obj_ids(obj1) if req.target_object_types: obj2_ids = any_obj_types_ids(obj2, req.target_object_types) else: obj2_ids = any_obj_ids(obj2) ids = db().query( obj1.id, obj2.id ).\ filter(obj1.id.in_( obj1_ids ), obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \ obj2.id.in_( obj2_ids ), obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox", \ SFCGAL_Contains3D( geo1.geometry, geo2.geometry) ).all() for i1, i2 in ids: pair = ObjectPair() pair.reference_id = i1 pair.relations.append("contains") pair.target_id = i2 res.target_object_ids = [id for id, in ids] return res
def get_objects_within_area(req): rospy.loginfo("SEMAP DB SRVs: get_objects_within_area") res = GetObjectsWithinAreaResponse() tar_obj = aliased(ObjectInstance) tar_geo = aliased(GeometryModel) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.fully_within: operator = ST_Within(tar_geo.geometry, fromPolygon2D(req.reference_polygon)) else: operator = or_( ST_Overlaps(fromPolygon2D(req.reference_polygon), tar_geo.geometry), ST_Within(tar_geo.geometry, fromPolygon2D(req.reference_polygon))) results = db().query( tar_obj.id ).\ filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).all() for i in results: pair = ObjectPair() pair.reference_id = -1 pair.target_id = i[0] pair.relations.append("contained-in-area") res.pairs.append(pair) return res
def get_objects_within_range( req ): rospy.loginfo( "SEMAP DB SRVs: get_objects_within_range" ) res = GetObjectsWithinRangeResponse() tar_obj = aliased( ObjectInstance ) tar_geo = aliased( GeometryModel ) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.fully_within: operator = ST_3DDFullyWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance) else: operator = ST_3DDWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance) results = db().query( tar_obj.id, ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DMaxDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DClosestPoint(tar_geo.geometry, fromPoint3D(req.reference_point)) ).\ filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).\ order_by( ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry) ).all() for i, min, max, point in results: print i, min, max, point pair = ObjectPair() pair.reference_id = -1 pair.target_id = i pair.max_dist = max pair.max_dist_line[0] = req.reference_point pair.max_dist_line[1] = toPoint3D( point ) pair.min_dist = min pair.min_dist_line[0] = req.reference_point pair.min_dist_line[1] = toPoint3D( point ) res.pairs.append(pair) return res
def get_objects_within_range(req): rospy.loginfo("SEMAP DB SRVs: get_objects_within_range") res = GetObjectsWithinRangeResponse() tar_obj = aliased(ObjectInstance) tar_geo = aliased(GeometryModel) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.fully_within: operator = ST_3DDFullyWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance) else: operator = ST_3DDWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance) results = db().query( tar_obj.id, ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DMaxDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DClosestPoint(tar_geo.geometry, fromPoint3D(req.reference_point)) ).\ filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).\ order_by( ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry) ).all() for i, min, max, point in results: print i, min, max, point pair = ObjectPair() pair.reference_id = -1 pair.target_id = i pair.max_dist = max pair.max_dist_line[0] = req.reference_point pair.max_dist_line[1] = toPoint3D(point) pair.min_dist = min pair.min_dist_line[0] = req.reference_point pair.min_dist_line[1] = toPoint3D(point) res.pairs.append(pair) return res
def get_distance_between_objects( req ): rospy.loginfo( "SEMAP DB SRVs: get_distance_between_objects3d" ) res = GetDistanceBetweenObjectsResponse() ref_obj = aliased( ObjectInstance ) ref_geo = aliased( GeometryModel ) tar_obj = aliased( ObjectInstance ) tar_geo = aliased( GeometryModel ) if req.reference_object_geometry_type not in ["Position3D", "AxisAligned3D", "BoundingBox", "BoundingHull", "Body"]: rospy.logerr("SEMAP DB SRVs: get_distance_between_objects3d was called with %s which is not a valid 2D geometry type" % req.reference_object_geometry_type) else: if req.reference_object_types: ref_ids = any_obj_types_ids(ref_obj, req.reference_object_types) else: ref_ids = any_obj_ids(ref_obj) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.max_distance: distance = ST_3DMaxDistance(ref_geo.geometry, tar_geo.geometry) line = ST_3DLongestLine(ref_geo.geometry, tar_geo.geometry) else: distance = ST_3DDistance(ref_geo.geometry, tar_geo.geometry) line = ST_3DShortestLine(ref_geo.geometry, tar_geo.geometry) if req.return_points: query = db().query( ref_obj.id, tar_obj.id, distance, line).\ filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \ ref_obj.absolute_description_id == ref_geo.abstraction_desc, tar_obj.absolute_description_id == tar_geo.abstraction_desc, ref_geo.type == req.reference_object_geometry_type, tar_geo.type == req.target_object_geometry_type ) else: query = db().query( ref_obj.id, tar_obj.id, distance).\ filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \ ref_obj.absolute_description_id == ref_geo.abstraction_desc, tar_obj.absolute_description_id == tar_geo.abstraction_desc, ref_geo.type == req.reference_object_geometry_type, tar_geo.type == req.target_object_geometry_type ) if req.min_range: query = query.filter( distance > req.min_range ) if req.min_range: query = query.filter( distance < req.max_range ) if req.sort_descending: query = query.order_by( desc(distance) ) else: query = query.order_by( distance ) results = query.all() if req.return_points: for i, j, dist, points in results: pair = ObjectPair() pair.reference_id = i pair.target_id = j if req.max_distance: pair.max_dist = dist pair.max_dist_line[0] = toPoint3D( db().execute( ST_PointN( points, 1 ) ).scalar() ) pair.max_dist_line[1] = toPoint3D( db().execute( ST_PointN( points, 2 ) ).scalar() ) else: pair.min_dist = dist pair.min_dist_line[0] = toPoint3D( db().execute( ST_PointN( points, 1 ) ).scalar() ) pair.min_dist_line[1] = toPoint3D( db().execute( ST_PointN( points, 2 ) ).scalar() ) res.pairs.append(pair) else: for i, j, dist in results: pair = ObjectPair() pair.reference_id = i pair.target_id = j if req.max_distance: pair.max_dist = dist else: pair.min_dist = dist res.pairs.append(pair) 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_distance_between_objects(req): rospy.loginfo("SEMAP DB SRVs: get_distance_between_objects3d") res = GetDistanceBetweenObjectsResponse() ref_obj = aliased(ObjectInstance) ref_geo = aliased(GeometryModel) tar_obj = aliased(ObjectInstance) tar_geo = aliased(GeometryModel) if req.reference_object_geometry_type not in [ "Position3D", "AxisAligned3D", "BoundingBox", "BoundingHull", "Body" ]: rospy.logerr( "SEMAP DB SRVs: get_distance_between_objects3d was called with %s which is not a valid 2D geometry type" % req.reference_object_geometry_type) else: if req.reference_object_types: ref_ids = any_obj_types_ids(ref_obj, req.reference_object_types) else: ref_ids = any_obj_ids(ref_obj) if req.target_object_types: tar_ids = any_obj_types_ids(tar_obj, req.target_object_types) else: tar_ids = any_obj_ids(tar_obj) if req.max_distance: distance = ST_3DMaxDistance(ref_geo.geometry, tar_geo.geometry) line = ST_3DLongestLine(ref_geo.geometry, tar_geo.geometry) else: distance = ST_3DDistance(ref_geo.geometry, tar_geo.geometry) line = ST_3DShortestLine(ref_geo.geometry, tar_geo.geometry) if req.return_points: query = db().query( ref_obj.id, tar_obj.id, distance, line).\ filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \ ref_obj.absolute_description_id == ref_geo.abstraction_desc, tar_obj.absolute_description_id == tar_geo.abstraction_desc, ref_geo.type == req.reference_object_geometry_type, tar_geo.type == req.target_object_geometry_type ) else: query = db().query( ref_obj.id, tar_obj.id, distance).\ filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \ ref_obj.absolute_description_id == ref_geo.abstraction_desc, tar_obj.absolute_description_id == tar_geo.abstraction_desc, ref_geo.type == req.reference_object_geometry_type, tar_geo.type == req.target_object_geometry_type ) if req.min_range: query = query.filter(distance > req.min_range) if req.min_range: query = query.filter(distance < req.max_range) if req.sort_descending: query = query.order_by(desc(distance)) else: query = query.order_by(distance) results = query.all() if req.return_points: for i, j, dist, points in results: pair = ObjectPair() pair.reference_id = i pair.target_id = j if req.max_distance: pair.max_dist = dist pair.max_dist_line[0] = toPoint3D(db().execute( ST_PointN(points, 1)).scalar()) pair.max_dist_line[1] = toPoint3D(db().execute( ST_PointN(points, 2)).scalar()) else: pair.min_dist = dist pair.min_dist_line[0] = toPoint3D(db().execute( ST_PointN(points, 1)).scalar()) pair.min_dist_line[1] = toPoint3D(db().execute( ST_PointN(points, 2)).scalar()) res.pairs.append(pair) else: for i, j, dist in results: pair = ObjectPair() pair.reference_id = i pair.target_id = j if req.max_distance: pair.max_dist = dist else: pair.min_dist = dist res.pairs.append(pair) 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