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()