def test_polygon_model_functions(): point1 = ROSPoint32(0, 0, 0) point2 = ROSPoint32(1, 0, 1) point3 = ROSPoint32(1, 1, 0) point4 = ROSPoint32(0, 1, 1) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = 'point2dtest' polygon2dmodel.geometry.points.append(point1) polygon2dmodel.geometry.points.append(point2) polygon2dmodel.geometry.points.append(point3) polygon2dmodel.geometry.points.append(point4) geo2dmodel = GeometryModel() geo2dmodel.fromPolygon2DModel(polygon2dmodel) polygon3dmodel = Polygon3DModel() polygon3dmodel.type = 'polygon3dtest' polygon3dmodel.geometry.points.append(point1) polygon3dmodel.geometry.points.append(point2) polygon3dmodel.geometry.points.append(point3) polygon3dmodel.geometry.points.append(point4) geo3dmodel = GeometryModel() geo3dmodel.fromROSPolygon3DModel(polygon3dmodel) #db().add(geo2dmodel) #db().add(geo3dmodel) db().commit()
def constrain_obj_ids(obj, obj_constraint):
desc = aliased( ObjectDescription )
no_ids = False
no_class = False
no_super = False
query_constraint = None
if obj_constraint.ids and obj_constraint.ids != (0,):
query_constraint = obj.id.in_( obj_constraint.ids )
else:
no_ids = True
if obj_constraint.class_types and obj_constraint.class_types != ['']:
query_constraint = or_(query_constraint, or_(*[desc.type == type for type in obj_constraint.class_types]) )
else:
no_class = True
if obj_constraint.superclass_types and obj_constraint.superclass_types != ['']:
query_constraint = or_(query_constraint, or_(*[desc.type.contains(type) for type in obj_constraint.superclass_types]) )
else:
no_super = True
if no_ids and no_class and no_super:
return db().query( obj.id )
else:
return db().query( obj.id ).\
filter( obj.relative_description_id == desc.id,
query_constraint )
def set_geometry_model_pose(req):
rospy.loginfo("SpatialDB SRVs: set_geometry_model_pose")
res = SetTransformResponse()
model = db().query(GeometryModel).filter(GeometryModel.id == req.id).scalar()
model.pose.fromROS(req.pose)
db().commit()
return res
def set_transform(req):
rospy.loginfo("SpatialDB SRVs: set transform")
res = SetTransformResponse()
object = db().query(ObjectInstance).filter(ObjectInstance.id == req.id).scalar()
object.frame.setROSPose(req.pose)
db().commit()
return res
def update_geometry_model_pose(req):
rospy.loginfo("SpatialDB SRVs: update_geometry_model_pose")
res = UpdateGeometryModelPose()
model = db().query(GeometryModel).filter(GeometryModel.id == req.id).one()
model.pose.appendROSPose(req.pose)
db().commit()
return res
def update_transform(req):
rospy.loginfo("SpatialDB SRVs: update transform")
res = UpdateTransformResponse()
object = db().query(ObjectInstance).filter(ObjectInstance.id == req.id).one()
object.frame.appendROSPose(req.pose)
db().commit()
return res
def switch_object_descriptions(req):
res = SwitchObjectDescriptionsResponse()
objects = db().query(ObjectInstance).filter(ObjectInstance.id.in_(req.obj_ids)).all()
for obj in objects:
obj.object_description_id = req.desc_id
db().commit()
return res
def add_polygon_mesh_3d_model(req):
rospy.loginfo("SpatialDB SRVs: add_polygon_mesh_3d_model")
res = AddPolygonMesh3DModelResponse()
desc = db().query(ObjectDescription).filter(ObjectDescription.id==req.id).one()
desc.addPolygonMesh3DModel(req.model)
db().commit()
return res
def change_frame(req):
rospy.loginfo("SpatialDB SRVs: change frame")
res = ChangeFrameResponse()
object = db().query(ObjectInstance).filter(ObjectInstance.id == req.id).scalar()
object.frame.changeFrame(req.frame, req.keep_transform)
db().commit()
return res
def test_mesh_model_functions(): point1 = ROSPoint(0, 0, 0) point2 = ROSPoint(1, 0, 0) point3 = ROSPoint(1, 1, 0) point4 = ROSPoint(0, 1, 0) tri1 = ROSMeshTriangle() tri2 = ROSMeshTriangle() tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 1 tri1.vertex_indices[2] = 2 tri2.vertex_indices[0] = 0 tri2.vertex_indices[1] = 2 tri2.vertex_indices[2] = 3 mesh3dmodel = TriangleMesh3DModel() mesh3dmodel.type = 'mesh3d' mesh3dmodel.geometry.vertices.append(point1) mesh3dmodel.geometry.vertices.append(point2) mesh3dmodel.geometry.vertices.append(point3) mesh3dmodel.geometry.vertices.append(point4) mesh3dmodel.geometry.triangles.append(tri1) mesh3dmodel.geometry.triangles.append(tri2) geo3dmodel = GeometryModel() geo3dmodel.fromTriangleMesh3DModel(mesh3dmodel) db().add(geo3dmodel) db().commit()
def rename_geometry_model(req):
rospy.loginfo("SpatialDB SRVs: rename_geometry_model")
res = RenameGeometryModelResponse()
model = db().query(GeometryModel).filter(GeometryModel.id==req.id).one()
model.type = req.type
db().commit()
return res
def constrain_obj_ids(obj, obj_constraint):
desc = aliased(ObjectDescription)
no_ids = False
no_class = False
no_super = False
query_constraint = None
if obj_constraint.ids and obj_constraint.ids != (0, ):
query_constraint = obj.id.in_(obj_constraint.ids)
else:
no_ids = True
if obj_constraint.class_types and obj_constraint.class_types != ['']:
query_constraint = or_(
query_constraint,
or_(*[desc.type == type for type in obj_constraint.class_types]))
else:
no_class = True
if obj_constraint.superclass_types and obj_constraint.superclass_types != [
''
]:
query_constraint = or_(
query_constraint,
or_(*[
desc.type.contains(type)
for type in obj_constraint.superclass_types
]))
else:
no_super = True
if no_ids and no_class and no_super:
return db().query(obj.id)
else:
return db().query( obj.id ).\
filter( obj.relative_description_id == desc.id,
query_constraint )
def create_a_dummy_object_instance():
geo2d = GeometryModel()
geo2d.type = 'position2d'
geo2d.geometry_type = 'POINT'
geo2d.geometry = WKTElement('POINT(0 0)')
geo3d = GeometryModel()
geo3d.type = 'primitive3d'
geo3d.geometry_type = 'POINTZ'
#geo3d.geometry_type = 'LINESTRINGZ'
#geo3d.geometry_type = 'POLYGONZ'
geo3d.geometry = WKTElement('POINT(0 0 0)')
#geo3d.geometry = WKTElement('LINESTRING Z (2 2 6,1.5 1.5 7,1 1 8,0.5 0.5 8,0 0 10)')
#geo3d.geometry = WKTElement('POLYGON((0 0 8, 0 1 8, 1 1 8, 1 0 8, 0 0 8))')
obj_desc = ObjectDescription()
obj_desc.type = 'test_object'
obj_desc.geometry_model2d = geo2d
obj_desc.geometry_model3d = geo3d
pose = Pose()
pose.ref_system = 'origin'
pose.pose = '1,2,1,0,0,0,1'
obj_inst = ObjectInstance()
obj_inst.alias = '6'
obj_inst.pose = pose
obj_inst.object_description = obj_desc
db().add(obj_inst)
db().commit()
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 rename_object_instance( req ): rospy.loginfo( "SEMAP DB SRVs: rename_object_instance" ) res = RenameObjectInstanceResponse() inst = db().query( ObjectInstance ).filter( ObjectInstance.id==req.id ).one() inst.alias = req.alias db().commit() return res
def delete_object_descriptions(req):
rospy.loginfo("SpatialDB SRVs: delete_object_descriptions")
res = DeleteObjectDescriptionsResponse()
descs = db().query(ObjectDescription).filter(ObjectDescription.id.in_(req.ids)).all()
if len(descs) > 0:
for desc in descs:
inst_ids = desc.getInstancesIDs()
print inst_ids
if len(inst_ids) > 0:
res.ids += inst_ids
if not req.keep_instances:
child_req = DeleteObjectInstancesRequest()
child_req.ids = inst_ids
child_req.keep_children = True
child_res = delete_object_instances(child_req)
res.ids += child_res.ids
else:
if req.new_desc_id != 0:
request = SwitchObjectDescriptionsRequest()
request.obj_ids = inst_ids
request.desc_id = req.new_desc_id
switch_object_descriptions(request)
else:
objects = db().query(ObjectInstance).filter(ObjectInstance.id.in_(inst_ids)).all()
for obj in objects:
obj.description_id = None
for model in desc.geometry_models:
db().delete(model.pose)
db().delete(model)
db().delete(desc)
db().commit()
return res
def get_objects_within_range2d( req ):
rospy.loginfo( "SEMAP DB SRVs: get_objects_within_range2d" )
res = GetObjectsWithinRange2DResponse()
obj = aliased( ObjectInstance )
geo = aliased( GeometryModel )
print req.object_types, req.point, req.geometry_type, req.distance
if req.geometry_type not in ["Position2D", "AxisAligned2D", "FootprintBox", "FootprintHull"]:
rospy.logerr("SEMAP DB SRVs: get_objects_within_range2d was called with %s which is not a valid 2D geometry type" % req.geometry_type)
else:
rospy.loginfo("SEMAP DB SRVs: get_objects_within_range2d tries to find em")
if req.object_types:
obj_ids = any_obj_types_ids(obj, req.object_types)
else:
obj_ids = any_obj_ids(obj)
if req.fully_within:
ids = db().query( obj.id ).filter( obj.id.in_( obj_ids ), \
obj.absolute_description_id == geo.abstraction_desc, geo.type == req.geometry_type, \
ST_DFullyWithin(fromPoint2D(req.point),geo.geometry, req.distance)
).all()
else:
ids = db().query( obj.id ).filter( obj.id.in_( obj_ids ), \
obj.absolute_description_id == geo.abstraction_desc, geo.type == req.geometry_type, \
ST_DWithin(fromPoint2D(req.point), geo.geometry, req.distance)
).all()
res.ids = [id for id, in ids]
return res
def test_containment_relations3d( req ):
rospy.loginfo( "SEMAP DB SRVs: test_containment_relations3d" )
res = GetDirectionalRelations2DResponse()
obj1 = aliased( ObjectInstance )
geo1 = aliased( GeometryModel )
obj2 = aliased( ObjectInstance )
geo2 = aliased( GeometryModel )
geos = db().query( SFCGAL_Contains3D( geo1.geometry, geo2.geometry) ).\
filter(obj1.id == req.reference_id, \
obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \
obj2.id == req.target_id, \
obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox" ).all()
print geos
for geoI, geoII in geos:
print 'geoI', db().execute( ST_AsText( geoI.geometry) ).scalar()
#print 'isValid', db().execute( SFCGAL_IsValid( geoI.geometry) ).scalar()
print 'geoI', db().execute( ST_AsText( geoII.geometry) ).scalar()
#print 'isValid', db().execute( SFCGAL_IsValid( geoII.geometry) ).scalar()
containment_status = db().execute( SFCGAL_Contains3D( geoI.geometry, geoII.geometry ) ).scalar()
print 'containment_status:', containment_status
#if containment_status:
# rospy.loginfo("OBJECT CONTAINMENT VERIFIED!")
#else:
# rospy.loginfo("OBJECT CONTAINMENT REJECTED!")
return res
def test_containment_relations3d(req):
rospy.loginfo("SEMAP DB SRVs: test_containment_relations3d")
res = GetDirectionalRelations2DResponse()
obj1 = aliased(ObjectInstance)
geo1 = aliased(GeometryModel)
obj2 = aliased(ObjectInstance)
geo2 = aliased(GeometryModel)
geos = db().query( SFCGAL_Contains3D( geo1.geometry, geo2.geometry) ).\
filter(obj1.id == req.reference_id, \
obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \
obj2.id == req.target_id, \
obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox" ).all()
print geos
for geoI, geoII in geos:
print 'geoI', db().execute(ST_AsText(geoI.geometry)).scalar()
#print 'isValid', db().execute( SFCGAL_IsValid( geoI.geometry) ).scalar()
print 'geoI', db().execute(ST_AsText(geoII.geometry)).scalar()
#print 'isValid', db().execute( SFCGAL_IsValid( geoII.geometry) ).scalar()
containment_status = db().execute(
SFCGAL_Contains3D(geoI.geometry, geoII.geometry)).scalar()
print 'containment_status:', containment_status
#if containment_status:
# rospy.loginfo("OBJECT CONTAINMENT VERIFIED!")
#else:
# rospy.loginfo("OBJECT CONTAINMENT REJECTED!")
return res
def rename_object_description(req):
rospy.loginfo("SpatialDB SRVs: rename_object_description")
res = RenameObjectDescriptionResponse()
desc = db().query(ObjectDescription).filter(ObjectDescription.id==req.id).one()
desc.type = req.type
db().commit()
return res
def rename_object_instance(req):
rospy.loginfo("SEMAP DB SRVs: rename_object_instance")
res = RenameObjectInstanceResponse()
inst = db().query(ObjectInstance).filter(ObjectInstance.id == req.id).one()
inst.alias = req.alias
db().commit()
return res
def test_retrieval(req):
rospy.loginfo("SpatialDB SRVs: test_distance")
then = rospy.Time.now()
origin = WKTElement('POINT(%f %f %f)' % (0.0, 0.0, 0.0))
geo0 = aliased(GeometryModel)
geo1 = aliased(GeometryModel)
obj1 = aliased(ObjectInstance)
obj2 = aliased(ObjectInstance)
anyobj = db().query( geo0.geometry ).filter( geo0.type == "Body", obj1.absolute_description_id == geo0.object_description_id ).label( "any" )
res0 = db().query( geo0.geometry ).filter( obj1.id == 2, geo0.type == "Body", obj1.absolute_description_id == geo0.object_description_id ).label( "res0" )
res1 = db().query( geo1.geometry ).filter( obj2.id == 3, geo1.type == "Body", obj2.absolute_description_id == geo1.object_description_id ).label( "res1" )
root_dist = SFCGAL_Distance3D( origin, res0 ).label("root_dist")
in_root_range = db().query( obj1.id, root_dist ).filter(root_dist > 2.0)
print in_root_range.all()
#obj_dist = SFCGAL_Distance3D( res0, res1 ).label("obj_dist")
#in_obj_range = db().query( obj1.id, obj2.id, obj_dist ).filter(obj_dist > 0.0)
#print in_obj_range.all()
intersects = db().query( obj1.id, obj2.id, SFCGAL_Intersects3D( res0, res1 ) ).filter(obj1.id != obj2.id)
print intersects.all()
#dist = db().query( obj1.id, ).filter(SFCGAL_Distance3D( WKTElement('POINT(%f %f %f)' % (0.0, 0.0, 0.0)), res0 ) > 2.0)#.label("dist")
#inrange = db().query( obj1 ).filter( SFCGAL_Distance3D( WKTElement('POINT(%f %f %f)' % (0.0, 0.0, 0.0)), res0 ) > 2.0 )#.label("dist")
#inrange = db().query( obj1.id).filter( dist > 0.0).all()
#print dist
#for o in dist:
# print o
#resres0 = db().query(geo0.geometry).select_from(join(geo0, ObjectInstance)).filter( ObjectInstance.id == 3, ObjectInstance.absolute_description_id == GeometryModel.object_description_id).all()
#
#print 'frist', len(resres0)
#for g in resres0:
#print db().execute( ST_AsText(g) ).scalar()
#print i.name, g.type
#print 'second'
#for i, g in res1:
#print i.name, g.type
#print 'disttest', db().query( ST_Distance( resres0, WKTElement('POINT(1.0 0.0 0.0)') ) ).scalar()
# laeuft
## print 'disttest', db().query(ObjectInstance.id).filter( ST_Distance( ObjectInstance.tester(), WKTElement('POINT(1.0 0.0 0.0)') ) > 0).all()
#print 'disttest', db().query(ObjectInstance.id).filter( ST_Distance( ObjectInstance.tester2(), WKTElement('POINT(1.0 0.0 0.0)') ) > 0).all()
#print 'disttest', db().query(ObjectInstance.id).filter(ST_Distance( WKTElement('POINT(1.0 0.0 0.0)'), WKTElement('POINT(1.0 0.0 0.0)') ) > 0).all()
#obj1.id, obj2.id).filter( db().execute( ST_DWithin( obj1.getAPosition2D() , obj2.getAPosition2D(), 20.0) ) ): #ST_DWithin( obj1.getAPosition2D() , obj2.getAPosition2D(), 2.0 )
#dat = db().exists().where( db().execute( ST_Within( origin , ObjectInstance.getAPosition2D() ) ) )
#obj_within_range = db().query(ObjectInstance).filter(ST_Within( origin , ObjectInstance.getAPosition2D) ).all()
## print 'disttest', db().query(ObjectInstance, ObjectInstance.frame).filter( ST_Distance( ObjectInstance.tester2(), WKTElement('POINT(1.0 0.0 0.0)') ) > 0).all()
return GetObjectInstancesResponse()
def add_polygon_mesh_3d_model( req ): rospy.logdebug( "SEMAP DB SRVs: add_polygon_mesh_3d_model" ) res = AddPolygonMesh3DModelResponse() desc = db().query( ObjectDescription ).filter( ObjectDescription.id == req.id ).one() res.id = desc.addPolygonMesh3DModel( req.model ) db().commit() call_update_object_descriptions( [ req.id ] ) return res
def remove_geometry_model(req):
rospy.loginfo("SpatialDB SRVs: remove_geometry_model")
res = RemoveGeometryModelResponse()
print 'desc id', req.id, 'model type', req.type
model = db().query(GeometryModel).filter(GeometryModel.object_description_id==req.id, GeometryModel.type==req.type).one()
db().delete(mo)
db().commit()
return res
def add_triangle_mesh_3d_model(req):
rospy.loginfo("SpatialDB SRVs: add_triangle_mesh_3d_model")
then = rospy.Time.now()
res = AddTriangleMesh3DModelResponse()
desc = db().query(ObjectDescription).filter(ObjectDescription.id==req.id).one()
desc.addTriangleMesh3DModel(req.model)
db().commit()
rospy.loginfo("Took %f seconds" % (rospy.Time.now() - then).to_sec())
return res
def set_geometry_model_pose( req ): rospy.logdebug( "SEMAP DB SRVs: set_geometry_model_pose" ) res = SetGeometryModelPoseResponse() model = db().query( GeometryModel ).filter( GeometryModel.id == req.id ).one() model.pose.fromROS( req.pose ) db().commit() update_res = call_update_object_descriptions( [ model.geometry_desc ] ) res.ids = update_res.ids return res
def switch_object_descriptions( req ):
rospy.loginfo( "SEMAP DB SRVs: switch_object_descriptions" )
res = SwitchObjectDescriptionsResponse()
objects = db().query( ObjectInstance ).filter( ObjectInstance.id.in_( req.obj_ids ) ).all()
for obj in objects:
obj.relative_description_id = req.desc_id
db().commit()
call_update_absolute_descriptions(req.obj_ids)
return res
def set_transform( req ): rospy.loginfo( "SEMAP DB SRVs: set transform" ) res = SetTransformResponse() object = db().query( ObjectInstance ).filter(ObjectInstance.id == req.id).scalar() object.frame.setROSPose(req.pose) db().commit() update_res = call_update_absolute_descriptions( [ req.id ] ) res.ids = update_res.ids return res
def remove_geometry_model( req ): rospy.logdebug( "SEMAP DB SRVs: remove_geometry_model" ) res = RemoveGeometryModelResponse() model = db().query( GeometryModel ).filter( GeometryModel.id == req.id ).one() db().delete( model ) db().commit() update_res = call_update_object_descriptions( [ model.geometry_desc ] ) res.ids = update_res.ids return res
def rename_object_description( req ): rospy.loginfo( "SEMAP DB SRVs: rename_object_description" ) res = RenameObjectDescriptionResponse() desc = db().query( ObjectDescription ).filter( ObjectDescription.id == req.id ).one() desc.type = req.type db().commit() update_res = call_update_object_descriptions( [ req.id ] ) res.ids = update_res.ids return res
def update_and_transform_geometry_model_pose(req):
rospy.logdebug("SEMAP DB SRVs: update_and_transform_geometry_model_pose")
res = UpdateGeometryModelPoseResponse()
model = db().query(GeometryModel).filter(GeometryModel.id == req.id).one()
model.applyROSPose(req.pose)
db().commit()
update_res = call_update_object_descriptions([model.geometry_desc])
res.ids = update_res.ids
return res
def set_geometry_model_pose(req):
rospy.logdebug("SEMAP DB SRVs: set_geometry_model_pose")
res = SetGeometryModelPoseResponse()
model = db().query(GeometryModel).filter(GeometryModel.id == req.id).one()
model.pose.fromROS(req.pose)
db().commit()
update_res = call_update_object_descriptions([model.geometry_desc])
res.ids = update_res.ids
return res
def remove_geometry_model(req):
rospy.logdebug("SEMAP DB SRVs: remove_geometry_model")
res = RemoveGeometryModelResponse()
model = db().query(GeometryModel).filter(GeometryModel.id == req.id).one()
db().delete(model)
db().commit()
update_res = call_update_object_descriptions([model.geometry_desc])
res.ids = update_res.ids
return res
def add_triangle_mesh_3d_model( req ): rospy.logdebug( "SEMAP DB SRVs: add_triangle_mesh_3d_model" ) then = rospy.Time.now() res = AddTriangleMesh3DModelResponse() desc = db().query( ObjectDescription ).filter( ObjectDescription.id == req.id ).one() res.id = desc.addTriangleMesh3DModel( req.model ) db().commit() call_update_object_descriptions( [ req.id ] ) return res
def update_and_transform_geometry_model_pose( req ): rospy.logdebug( "SEMAP DB SRVs: update_and_transform_geometry_model_pose" ) res = UpdateGeometryModelPoseResponse() model = db().query( GeometryModel ).filter( GeometryModel.id == req.id ).one() model.applyROSPose(req.pose) db().commit() update_res = call_update_object_descriptions( [ model.geometry_desc ] ) res.ids = update_res.ids return res
def add_polygon_mesh_3d_model(req):
rospy.logdebug("SEMAP DB SRVs: add_polygon_mesh_3d_model")
res = AddPolygonMesh3DModelResponse()
desc = db().query(ObjectDescription).filter(
ObjectDescription.id == req.id).one()
res.id = desc.addPolygonMesh3DModel(req.model)
db().commit()
call_update_object_descriptions([req.id])
return res
def change_frame( req ):
rospy.logdebug( "SEMAP DB SRVs: change frame" )
res = ChangeFrameResponse()
object = db().query( ObjectInstance ).filter( ObjectInstance.id == req.id ).scalar()
object.frame.changeFrame( req.frame, req.keep_transform )
db().commit()
if req.keep_transform:
update_res = call_update_absolute_descriptions( [ req.id ] )
res.ids = update_res.ids
return res
def rename_object_description(req):
rospy.loginfo("SEMAP DB SRVs: rename_object_description")
res = RenameObjectDescriptionResponse()
desc = db().query(ObjectDescription).filter(
ObjectDescription.id == req.id).one()
desc.type = req.type
db().commit()
update_res = call_update_object_descriptions([req.id])
res.ids = update_res.ids
return res
def add_triangle_mesh_3d_model(req):
rospy.logdebug("SEMAP DB SRVs: add_triangle_mesh_3d_model")
then = rospy.Time.now()
res = AddTriangleMesh3DModelResponse()
desc = db().query(ObjectDescription).filter(
ObjectDescription.id == req.id).one()
res.id = desc.addTriangleMesh3DModel(req.model)
db().commit()
call_update_object_descriptions([req.id])
return res
def switch_object_descriptions(req):
rospy.loginfo("SEMAP DB SRVs: switch_object_descriptions")
res = SwitchObjectDescriptionsResponse()
objects = db().query(ObjectInstance).filter(
ObjectInstance.id.in_(req.obj_ids)).all()
for obj in objects:
obj.relative_description_id = req.desc_id
db().commit()
call_update_absolute_descriptions(req.obj_ids)
return res
def set_transform(req):
rospy.loginfo("SEMAP DB SRVs: set transform")
res = SetTransformResponse()
object = db().query(ObjectInstance).filter(
ObjectInstance.id == req.id).scalar()
object.frame.setROSPose(req.pose)
db().commit()
update_res = call_update_absolute_descriptions([req.id])
res.ids = update_res.ids
return res
def delete_all_absolute_descriptions(req):
rospy.logdebug("SEMAP DB SRVs: delete_all")
req = UpdateObjectDescriptionsRequest()
res = UpdateObjectDescriptionsResponse()
descs = db().query(ObjectDescription).filter(
ObjectDescription.type.match('absolute_description_')).all()
for desc in descs:
print 'Delete', desc.type
db().delete(desc)
db().commit()
return res
def change_frame(req):
rospy.logdebug("SEMAP DB SRVs: change frame")
res = ChangeFrameResponse()
object = db().query(ObjectInstance).filter(
ObjectInstance.id == req.id).scalar()
object.frame.changeFrame(req.frame, req.keep_transform)
db().commit()
if req.keep_transform:
update_res = call_update_absolute_descriptions([req.id])
res.ids = update_res.ids
return res
