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
