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_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
