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;
}
