def test_object_instance_insertion(): point2dmodel = Point2DModel() point2dmodel.type = '2dpoint' point2dmodel.geometry.x = 17.0 point2dmodel.geometry.y = 4.0 point2dmodel.geometry.z = 11.0 pose2dmodel = Pose2DModel() pose2dmodel.type = '2dpose' pose2dmodel.pose = ROSPose2D() pose2dmodel.pose.x = 0.5 pose2dmodel.pose.y = 1.5 pose2dmodel.pose.theta = 0.5 point0 = ROSPoint32(0, 0, 0) point1 = ROSPoint32(1, 0, 1) point2 = ROSPoint32(1, 1, 0) point3 = ROSPoint32(0, 1, 1) point4 = ROSPoint32(-1, 0, 1) point5 = ROSPoint32(-1, 1, 1) polygon0 = ROSPolygon() polygon0.points.append(point0) polygon0.points.append(point1) polygon0.points.append(point2) polygon0.points.append(point3) polygon1 = ROSPolygon() polygon1.points.append(point4) polygon1.points.append(point0) polygon1.points.append(point3) polygon1.points.append(point5) polygon2dmodel = Polygon2DModel() polygon2dmodel.type = '2dpolygon' polygon2dmodel.pose = ROSPose() polygon2dmodel.geometry = polygon0 point3dmodel = Point3DModel() point3dmodel.type = '3dpoint' point3dmodel.geometry.x = 19.0 point3dmodel.geometry.y = 66.0 point3dmodel.geometry.z = 12.0 pose3dmodel = Pose3DModel() pose3dmodel.type = '3dpose' pose3dmodel.pose = ROSPose() pose3dmodel.pose.position.x = 15.0 polygon3dmodel = Polygon3DModel() polygon3dmodel.type = '3dpolygon' polygon3dmodel.pose = ROSPose() polygon3dmodel.pose.position.x = 6.0 polygon3dmodel.geometry = polygon1 tri0 = ROSMeshTriangle() tri1 = ROSMeshTriangle() tri0.vertex_indices[0] = 0 tri0.vertex_indices[1] = 1 tri0.vertex_indices[2] = 2 tri1.vertex_indices[0] = 0 tri1.vertex_indices[1] = 2 tri1.vertex_indices[2] = 3 trianglemesh3dmodel = TriangleMesh3DModel() trianglemesh3dmodel.type = '3dtrianglemesh' trianglemesh3dmodel.pose = ROSPose() trianglemesh3dmodel.geometry.vertices.append(point0) trianglemesh3dmodel.geometry.vertices.append(point1) trianglemesh3dmodel.geometry.vertices.append(point2) trianglemesh3dmodel.geometry.vertices.append(point3) trianglemesh3dmodel.geometry.triangles.append(tri0) trianglemesh3dmodel.geometry.triangles.append(tri1) polygonmesh3dmodel = PolygonMesh3DModel() polygonmesh3dmodel.type = '3dpolygonmesh' polygonmesh3dmodel.pose = ROSPose() rand_quat = random_quaternion() polygonmesh3dmodel.pose.orientation.x = rand_quat[0] polygonmesh3dmodel.pose.orientation.y = rand_quat[1] polygonmesh3dmodel.pose.orientation.z = rand_quat[2] polygonmesh3dmodel.pose.orientation.w = rand_quat[3] polygonmesh3dmodel.geometry.polygons.append(polygon0) polygonmesh3dmodel.geometry.polygons.append(polygon1) desc_ros = ROSObjectDescription() desc_ros.type = "test_description" desc_ros.point2d_models.append(point2dmodel) desc_ros.pose2d_models.append(pose2dmodel) desc_ros.polygon2d_models.append(polygon2dmodel) desc_ros.point3d_models.append(point3dmodel) desc_ros.pose3d_models.append(pose3dmodel) desc_ros.polygon3d_models.append(polygon3dmodel) desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel) desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel) pose = ROSPoseStamped() pose.header.frame_id = "ref1" pose.pose.position.x = "0.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" pose.pose.orientation.x = "0.0" pose.pose.orientation.y = "0.0" pose.pose.orientation.z = "0.0" pose.pose.orientation.w = "1.0" inst_ros = ROSObjectInstance() inst_ros.alias = "mr_objecto" inst_ros.pose = pose inst_ros.description = desc_ros # create instance from ROS inst_db0 = ObjectInstance() inst_db0.fromROS(inst_ros) db().add(inst_db0) # create instance from existing model via id inst_db1 = ObjectInstance() inst_db1.object_description_id = 1 inst_db1.alias = "mrs_objecto" inst_db1.pose = GlobalPose() pose.pose.position.x = "1.0" pose.pose.position.y = "1.0" pose.pose.position.z = "0.0" inst_db1.pose.pose = fromROSPose(pose.pose) inst_db1.pose.ref_system = 'origin' #db().add(inst_db1) db().commit()
def test_point_model_functions():
a = Point3DModel()
a.type = 'object_point2d_1'
a.geometry.x = 17.0
a.geometry.y = 4.0
a.geometry.z = 11.0
b = Point3DModel()
b.type = 'object_point2d_2'
b.geometry.x = 17.0
b.geometry.y = 4.0
b.geometry.z = 12.0
c = Point3DModel()
c.type = 'object_point3d_3'
c.geometry.x = 19.0
c.geometry.y = 66.0
c.geometry.z = 12.0
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)
polygon3dmodel = Polygon3DModel()
polygon3dmodel.type = 'polygon3dtest3'
polygon3dmodel.geometry.points.append(point1)
polygon3dmodel.geometry.points.append(point2)
polygon3dmodel.geometry.points.append(point3)
polygon3dmodel.geometry.points.append(point4)
desc_ros = ROSObjectDescription()
desc_ros.type = "poly_test"
#desc_ros.point2d_models.append(a)
#desc_ros.polygon2d_models.append(polygon2dmodel)
#desc_ros.point2d_models.append(b)
#desc_ros.polygon2d_models.append(polygon2dmodel)
#desc_ros.polygon3d_models.append(polygon3dmodel)
for i in db().query(ObjectDescription).filter(ObjectDescription.id==4):
print i.type
new = GeometryModel()
new.fromROSPolygon3DModel(polygon3dmodel)
i.geometry_models3d.append(new)
db().commit()
print 'did it'
desc_db = ObjectDescription()
desc_db.fromROS(desc_ros)
#db().add(desc_db)
for desc in db().query(ObjectDescription):
print 'ObjectDescription #',desc.id, 'has type', desc.type
print 'and', len(desc.geometry_models2d), '2d models'
for model in desc.geometry_models2d:
print ' 2DModel #', model.id, model.type, model.geometry_type, model.object_description_id, model.object_description.type
print 'and', len(desc.geometry_models3d), '3d models'
for model in desc.geometry_models3d:
print ' 3DModel #', model.id, model.type, model.geometry_type, model.object_description_id, model.object_description.type
db().commit()
