void OgreMeshReader::readMesh(void) { OSG_OGRE_LOG(("OgreMeshReader::readMesh\n")); bool skelAnim = readBool(_is); bool stop = false; Int16 boneIdxVE = -1; Int16 boneWeightVE = -1; VertexElementStore sharedVertexElements; SubMeshStore subMeshInfo; while(_is) { readChunkHeader(_is); switch(_header.chunkId) { case CHUNK_SUBMESH: readSubMesh(subMeshInfo, sharedVertexElements, skelAnim); break; case CHUNK_GEOMETRY: readGeometry(sharedVertexElements); break; case CHUNK_MESH_SKELETON_LINK: readMeshSkeletonLink(subMeshInfo); break; case CHUNK_MESH_BONE_ASSIGNMENT: readMeshBoneAssignment(sharedVertexElements, boneIdxVE, boneWeightVE); break; case CHUNK_MESH_LOD: readMeshLOD(); break; case CHUNK_MESH_BOUNDS: readMeshBounds(); break; case CHUNK_SUBMESH_NAME_TABLE: readSubMeshNameTable(subMeshInfo); break; case CHUNK_EDGE_LISTS: readEdgeLists(); break; case CHUNK_POSES: readPoses(); break; case CHUNK_ANIMATIONS: readAnimations(); break; case CHUNK_TABLE_EXTREMES: readTableExtremes(); break; default: OSG_OGRE_LOG(("OgreMeshReader::readMesh: Unknown chunkId '0x%x'\n", _header.chunkId)); stop = true; break; }; if(stop == true) { skip(_is, -_chunkHeaderSize); break; } } if(boneIdxVE >= 0 || boneWeightVE >= 0) { verifyBoneAssignment(sharedVertexElements, boneIdxVE, boneWeightVE); } SubMeshStore::iterator smIt = subMeshInfo.begin(); SubMeshStore::iterator smEnd = subMeshInfo.end (); for(; smIt != smEnd; ++smIt) { if((*smIt).sharedVertex == true) { constructSubMesh(*smIt, sharedVertexElements); } else { constructSubMesh(*smIt, (*smIt).vertexElements); } } }
int main(int argc, char **argv) { ros::init(argc, argv, "amclfakeISL"); ros::NodeHandle n; ros::Publisher amclPosePublisher = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 1000); ros::Subscriber startInfoSubscriber = n.subscribe("communicationISL/neighborInfo",5, startInfoCallback); QString path = QDir::homePath(); path.append("/fuerte_workspace/sandbox/configISL.json"); if(!readConfigFile(path)){ qDebug()<< "Read Config File Failed!!!"; ros::shutdown(); return 0; } ros::Rate loop_rate(loopRate); path = QDir::homePath(); path.append("/fuerte_workspace/sandbox/poses.txt"); if(!readPoses(path)){ qDebug()<< "Read Pose File Failed!!!"; ros::shutdown(); return 0; } /* FILE *fp; fp = fopen("poses.txt","r"); if(fp==NULL) { std::cout <<"Error - could not open poses.txt"; return 0; } else { float x[numOfRobots+1], y[numOfRobots+1]; int i = 0; while (!feof(fp)) { fscanf(fp, "%f %f %f %f %f %f %f %f %f %f", &x[1], &y[1], &x[2], &y[2], &x[3], &y[3], &x[4], &y[4], &x[5], &y[5]); //poses[i][0] = t; poses[i][1] = x[robotID]/100; poses[i][2] = y[robotID]/100; //qDebug()<< poses[i][1]<<" "<<poses[i][2]; i = i + 1; } numOfPoses = i; } std::cout << numOfPoses; */ int timeIndx = 0; while (ros::ok()) { if (startPublishingPose) { if (timeIndx<numOfPoses) { robotPose.pose.pose.position.x = poses[timeIndx][1]; //in meters robotPose.pose.pose.position.y = poses[timeIndx][2]; //in meters robotPose.pose.pose.position.z = 0.1; robotPose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0); amclPosePublisher.publish(robotPose); } /* else { return 0; } */ timeIndx = timeIndx + increment; } ros::spinOnce(); loop_rate.sleep(); } return 0; }