def test_object_query():
o1 = aliased(ObjectInstance)
o2 = aliased(ObjectInstance)
for object1, object2 in \
db().query(o1, o2).\
filter(o1.alias=='5').\
filter(o2.alias=='6'):
print 'Object 1'
print 'Alias:', object1.alias
print 'Type:',object1.object_description.type
print '2D Model Type:',object1.object_description.geometry_model2d.type
print '2D Geo Type:',object1.object_description.geometry_model2d.geometry_type
print '2D Geo:', object1.object_description.geometry_model2d.geometry
print '3D Model Type:',object1.object_description.geometry_model3d.type
print '3D Geo Type::',object1.object_description.geometry_model3d.geometry_type
print '3D Geo:', object1.object_description.geometry_model3d.geometry
print 'Object 2'
print 'Alias:', object2.alias
print 'Type:',object2.object_description.type
print '2D Model Type:',object2.object_description.geometry_model2d.type
print '2D Geo Type:',object2.object_description.geometry_model2d.geometry_type
print '2D Geo:', object2.object_description.geometry_model2d.geometry
print '3D Model Type:',object2.object_description.geometry_model3d.type
print '3D Geo Type::',object2.object_description.geometry_model3d.geometry_type
print '3D Geo:', object1.object_description.geometry_model3d.geometry
for r in db().execute(ST_AsText(ST_3DIntersection(object1.object_description.geometry_model3d.geometry, object2.object_description.geometry_model3d.geometry))):
print r
for r in db().execute(ST_AsText(ST_Affine(object1.object_description.geometry_model3d.geometry, cos(pi()), -sin(pi()), 0, sin(pi()), cos(pi()), 0, 0, 0, 1, 0, 0, 0))):
print r
for r in db().execute(func.ST_Distance(object1.object_description.geometry_model2d.geometry, object2.object_description.geometry_model2d.geometry)):
print r
for r in db().execute(func.ST_3DDistance(object1.object_description.geometry_model3d.geometry, object2.object_description.geometry_model3d.geometry)):
print r
def test_geography_trigger(self):
mis_id = self.add_mis()
stop = new_stop()
stop.mis_id = mis_id
self.db_session.add(stop)
self.db_session.flush()
point = self.db_session.query(ST_AsText(
metabase.Stop.geog)).filter_by(id=stop.id).one()[0]
self.assertEqual(
point, u"POINT(%s %s)" % (stop.long, stop.lat),
"geog attribute not coherent with long/lat attributes")
logging.debug("POINT %s", point)
stop = self.db_session.query(metabase.Stop).filter_by(id=stop.id).one()
stop.long = 92.321269
self.db_session.flush()
point = self.db_session.query(ST_AsText(
metabase.Stop.geog)).filter_by(id=stop.id).one()[0]
self.assertEqual(
point, u"POINT(%s %s)" % (stop.long, stop.lat),
"geog attribute not coherent with long/lat attributes")
logging.debug("POINT %s", point)
stop = self.db_session.query(metabase.Stop).filter_by(id=stop.id).one()
stop.lat = 37.123123
self.db_session.flush()
point = self.db_session.query(ST_AsText(
metabase.Stop.geog)).filter_by(id=stop.id).one()[0]
self.assertEqual(
point, u"POINT(%s %s)" % (stop.long, stop.lat),
"geog attribute not coherent with long/lat attributes")
logging.debug("POINT %s", point)
def test_get_object_instances():
for inst in db().query(ObjectInstance):
desc = inst.object_description
print 'ObjectInstance #',inst.id, 'has alias', inst.alias
print 'Pose: ', inst.pose
print 'ObjectDescription #',desc.id, 'has type', desc.type
print 'and', len(desc.geometry_models), 'geometry models'
for model in desc.geometry_models:
print ' Model #', model.id, model.type, model.geometry_type
print ' ', model.pose.pose
print ' ', db().execute(ST_AsText(model.geometry)).scalar()
print
print ' ', db().execute(ST_AsText(model.pose.apply(model.geometry))).scalar()
def getConvexHull2D(self, as_text=False):
if not as_text:
return db().execute(SFCGAL_Convexhull(
self.getGeometryCollection())).scalar()
else:
return db().execute(
ST_AsText(SFCGAL_Convexhull(
self.getGeometryCollection(True)))).scalar()
def extract_points_old(rosmsg,
session,
xmin=False,
xmax=False,
ymin=False,
ymax=False,
altmin=False,
altmax=False):
"""Extract the positions of the robot in a brick given its spatial delimitation
Parameters
----------
rosmsg : ROSMsg subclass instance
The SQLAlcheny ORM Base instance to communicate with the corresponding table
in the Postgresql database.
session : sqlalchemy.orm.session.Session
SQLAlchemy Session instance to send queries with.
xmin, xmax, ymin, ymax, altmin, altmax : float
Spatial boundaries of the brick to extract.
Returns
-------
points : ndarray, (N,3)
Positions of the robot in the given brick.
"""
xtotmin, xtotmax, ytotmin, ytotmax = session.query(\
func.min(rosmsg.lon),func.max(rosmsg.lon),\
func.min(rosmsg.lat),func.max(rosmsg.lat)).one()
alttotmin, alttotmax = session.query(\
func.min(rosmsg.alt), func.max(rosmsg.alt)).one()
x1, y1 = pyproj.transform(rosmsg.inProj(), rosmsg.outProj(), xtotmin,
ytotmin)
x2, y2 = pyproj.transform(rosmsg.inProj(), rosmsg.outProj(), xtotmax,
ytotmax)
# l1 = [xmin,xmax,ymin,ymax]
# l2 = [altmin, altmax]
# l3 = [x1,x2,y1,y2]
# l4 = [alttotmin, alttotmax]
# l1 = [l3[i] if not l1[i] else l1[i] for i in range(len(l1))]
# l2 = [l4[i] if not l2[i] else l2[i] for i in range(len(l2))]
l1 = smallest_list([xmin, xmax, ymin, ymax], [x1, x2, y1, y2])
l2 = smallest_list([altmin, altmax], [alttotmin, alttotmax])
print(l1, l2)
xmin, xmax, ymin, ymax = l1
altmin, altmax = l2
rect = create_rect(xmin=xmin, xmax=xmax, ymin=ymin, ymax=ymax)
sub = session.query(ST_AsText(rosmsg.pt), rosmsg.alt).\
filter(ST_Contains(rect,rosmsg.pt)).\
filter(rosmsg.alt>altmin).\
filter(rosmsg.alt<altmax).\
all()
nb_msgs = len(sub)
points = np.zeros([nb_msgs, 3])
for i in range(nb_msgs):
points[i, :] = float(sub[i][0][6:-1].split(" ")[0]), float(
sub[i][0][6:-1].split(" ")[1]), sub[i][1]
return points
def toPolygon3D(geometry, is_text=False):
if not is_text:
geometry = db().execute(ST_AsText(geometry)).scalar()
polygon = geometry.strip('POLYGON Z((').strip('))').split(',')
ros = Polygon()
for point in polygon[0:len(polygon) - 1]:
values = [float(x) for x in point.split(' ')]
ros.points.append(Point32(values[0], values[1], values[2]))
return ros
def getGeometryCollection(self, as_text=False):
models = []
if self.geometries:
for model in self.geometries:
models.append(model.transformed())
if not as_text:
return db().execute(ST_Collect(models)).scalar()
else:
return db().execute(ST_AsText(ST_Collect(models))).scalar()
def toPoint3D(geometry, is_text=False):
if not is_text:
geometry = db().execute(ST_AsText(geometry)).scalar()
values = [
float(x) for x in geometry.strip('POINT Z(').strip(')').split(' ')
]
point = Point()
point.x = values[0]
point.y = values[1]
point.z = values[2]
return point
def apply(self, geometry, as_text=False):
matrix = toMatrix(self.pose)
if not as_text:
return db().execute( ST_Affine(geometry, matrix[0][0], matrix[0][1], matrix[0][2], \
matrix[1][0], matrix[1][1], matrix[1][2], \
matrix[2][0], matrix[2][1], matrix[2][2], \
matrix[0][3], matrix[1][3], matrix[2][3]) ).scalar()
else:
return db().execute( ST_AsText( ST_Affine(geometry, matrix[0][0], matrix[0][1], matrix[0][2], \
matrix[1][0], matrix[1][1], matrix[1][2], \
matrix[2][0], matrix[2][1], matrix[2][2], \
matrix[0][3], matrix[1][3], matrix[2][3]) ) ).scalar()
def getConcaveHull2D(self, as_text=False):
# does not work at all
points = db().execute(ST_DumpPoints(
self.getGeometryCollection())).scalar()
for point in points:
print point
print points
coll = db().execute(ST_Collect(points)).scalar()
geo = db().execute(ST_ConcaveHull(coll, 0.99, False)).scalar()
print geo
if not as_text:
return geo
else:
return db().execute(
ST_AsText(ST_ConcaveHull(
self.getGeometryCollection(True)))).scalar()
def toPolygonMesh3D(geometry, is_text=False):
if not is_text:
geometry = db().execute(ST_AsText(geometry)).scalar()
mesh = PolygonMesh()
polygons = geometry.split(')),')
for polygon in polygons:
index = []
points = polygon.strip('POLYHEDRALSURFACE Z(((').strip('))').split(',')
for point in points[0:len(points) - 1]:
values = [float(x) for x in point.split()]
index.append(len(mesh.vertices))
ros_point = Point()
ros_point.x = values[0]
ros_point.y = values[1]
ros_point.z = values[2]
mesh.vertices.append(ros_point)
indices = PolygonIndices()
indices.vertex_indices = index
mesh.polygons.append(indices)
return mesh
def toTriangleMesh3D(geometry, is_text=False):
if not is_text:
geometry = db().execute(ST_AsText(geometry)).scalar()
mesh = TriangleMesh()
triangles = geometry.split(')),')
for triangle in triangles:
index = []
points = triangle.strip('TIN Z(((').strip('))').split(',')
for point in points[0:len(points) - 1]:
values = [float(x) for x in point.split()]
index.append(len(mesh.vertices))
point = Point()
point.x = values[0]
point.y = values[1]
point.z = values[2]
mesh.vertices.append(point)
indices = TriangleIndices()
indices.vertex_indices = index
mesh.triangles.append(indices)
return mesh
def extract_points(session, seq, x, y, z, pt, flg_proj=False, lims=False):
"""Extract the positions of the robot in a brick given its spatial delimitation
Parameters
----------
rosmsg : ROSMsg subclass instance
The SQLAlcheny ORM Base instance to communicate with the corresponding table
in the Postgresql database.
session : sqlalchemy.orm.session.Session
SQLAlchemy Session instance to send queries with.
xmin, xmax, ymin, ymax, altmin, altmax : float
Spatial boundaries of the brick to extract.
Returns
-------
points : ndarray, (N,3)
Positions of the robot in the given brick.
seq : SQLAlchemy Column
Column of primary keys of the selected points.
l1 : List of ints or floats
List of the limits wrt x and y axis of the selected data.
"""
if lims is not False:
xmin, xmax, ymin, ymax, zmin, zmax = lims
else:
xmin, xmax, ymin, ymax, zmin, zmax = False, False, False, False, False, False
xtotmin, xtotmax, ytotmin, ytotmax, ztotmin, ztotmax = session.query(\
func.min(x),func.max(x),\
func.min(y),func.max(y),\
func.min(z),func.max(z)).one()
# xtotmin, xtotmax, ytotmin, ytotmax, ztotmin, ztotmax = [-np.inf, np.inf, -np.inf, np.inf, -np.inf, np.inf]
print(xmin, xmax, ymin, ymax)
if flg_proj:
xmin, ymin = proj_xy(xmin, ymin)
xmax, ymax = proj_xy(xmax, ymax)
# l1 = [xmin,xmax,ymin,ymax]
# l2 = [zmin, zmax]
# l3 = [xtotmin,xtotmax,ytotmin,ytotmax]
# l4 = [ztotmin, ztotmax]
# l1 = [l3[i] if not l1[i] else l1[i] for i in range(len(l1))]
# l2 = [l4[i] if not l2[i] else l2[i] for i in range(len(l2))]
l1 = smallest_list([xmin, xmax, ymin, ymax],
[xtotmin, xtotmax, ytotmin, ytotmax])
l2 = smallest_list([zmin, zmax], [ztotmin, ztotmax])
print("\n\n\n", l1, l2, zmin, zmax, ztotmin, ztotmax, "\n\n\n")
l1 = [float(i) for i in l1]
l2 = [float(i) for i in l2]
xmin, xmax, ymin, ymax = l1
zmin, zmax = l2
print("\n\n\n", l1, l2, "\n\n\n")
rect = create_rect(xmin=xmin, xmax=xmax, ymin=ymin, ymax=ymax)
sub0 = session.query(seq, ST_AsText(pt), z).\
filter(ST_Contains(rect,pt)).\
filter(z>zmin).\
filter(z<zmax).\
all()
sub0 = np.array(sub0)
print("\n\n\n", sub0, "\n\n\n")
seq = sub0[:, 0]
sub = sub0[:, 1:]
nb_msgs = len(sub)
points = np.zeros([nb_msgs, 3])
for i in range(nb_msgs):
points[i, :] = float(sub[i, 0][6:-1].split(" ")[0]), float(
sub[i, 0][6:-1].split(" ")[1]), sub[i, 1]
l1
return points, seq, [xmin, xmax, ymin, ymax, zmin, zmax]