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
