void readShape(ifstream* file, Model* m){
readVertices(file, m);
skipLine(file); // Skip # verts
skipLine(file); // Skip blank line
skipLine(file); // Skip object name
readFaces(file, m);
skipLine(file); // Skip # faces
}
int main(int argc, char** argv) {
char* filein, fileout;
char buf[BUFSIZE];
FILE* fin;
FILE* fout;
int host, robot;
char interface[128];
int index, type, subtype;
struct PlayerHeader header;
if (argc != 3) {
fprintf(stderr, "Syntax: player2dpslam <filein> <fileout>");
return 1;
}
fin = fopen(argv[1], "r");
if (fin == NULL) {
fprintf(stderr, "ERROR opening input file %s\n", argv[1]);
return 1;
}
fout = fopen(argv[2], "w");
if (fout == NULL) {
fprintf(stderr, "ERROR opening output file %s\n", argv[2]);
return 1;
}
while (!feof(fin)) {
header.readHeader(fin);
//header.Print();
if (header.valid) {
if (strcmp(header.interfaceName, "laser") == 0) {
if (state == WAITING_LASER) {
outputLaser(fin, fout);
//skipLine(fin);
state = WAITING_ODOMETRY;
}
else
skipLine(fin);
}
else if (strcmp(header.interfaceName, "position2d") == 0) {
if (state == WAITING_ODOMETRY) {
outputOdometry(fin, fout);
state = WAITING_LASER;
}
else
skipLine(fin);
}
}
fflush(fout);
}
fflush(fout);
return 0;
}
//parses a reading response
void hokuyo_parseReading(HokuyoRangeReading* r, char* buffer){
char* s=buffer;
int expectedStatus=0;
if (s[0]=='M')
expectedStatus=99;
if (s[0]=='C')
expectedStatus=00;
int beamBytes=0;
if (s[1]=='D') beamBytes=3;
if (s[0]=='C') beamBytes=3;
if (! beamBytes || ! expectedStatus){
fprintf(stderr, "Invalid return packet, cannot parse reading\n");
r->status=-1;
return;
}
s+=2;
char v[5];
v[4]=0;
strncpy(v,s,4); r->startStep=atoi(v); s+=4;
strncpy(v,s,4); r->endStep=atoi(v); s+=4;
v[2]=0; strncpy(v,s,2); r->clusterCount=atoi(v);
s=skipLine(s);
if (s==0){
fprintf(stderr, "error, line broken when reading the range parameters\n");
r->status=-1;
return;
}
strncpy(v,s,2); r->status=atoi(v); s+=2;
if (r->status==expectedStatus){
} else {
fprintf(stderr,"Error, Status=%d",r->status);
return;
}
r->timestamp=parseInt(4,&s);
s=skipLine(s);
int i=0;
while(s!=0){
r->ranges[i++]=parseInt(beamBytes,&s);
}
i--;
r->n_ranges=i;
}
void outputLaser(PlayerHeader h, FILE* fin, FILE* fout) {
//Expected values:
//-1.5708 +1.5708 +0.01745329 +5.6000 0181
double startangle, endangle, increment, minrange, maxrange;
unsigned int bins;
fscanf(fin, "%lf%lf%lf%lf%lf%u", &minrange, &startangle, &endangle, &increment, &maxrange, &bins);
//printf("startangle=%lf endangle=%lf increment=%lf minrange=%lf maxrange=%lf bins=%u\n", startangle, endangle, increment, minrange, maxrange, bins);
if (bins != 181) {
fprintf(stderr, "Warning: Expected 181 bins, found %i; skipping line\n\n", bins);
skipLine(fin);
state = WAITING_LASER;
return;
}
//1053653857.29 fly 6665 laser 00 1053652583.773 -1.571 +1.571 0.017453 181
fprintf(fout, "%lf %u %u %s %u %u %u ", h.time, h.host, h.robot, h.interfaceName, h.index, h.type, h.subtype);
fprintf(fout, "%lf %lf %lf %lf %lf %u ", minrange, startangle, endangle, increment, maxrange, bins);
int counter = 0;
char scanrange[128];
char intensity[128];
while (counter < 181) {
fscanf(fin, "%128s", scanrange);
fprintf(fout, "%s ", scanrange);
fscanf(fin, "%128s", intensity);
fprintf(fout, "%s ", intensity);
counter++;
}
fprintf(fout, "\n");
fflush(fout);
}
/* From DP_SLAM README, the output format needs to be:
LASER <number> <values>...
<number> is the number of laser readings that were observed. This
should usually be 181. Those actual laser measurements are the values
that follow, in meters. The laser readings are assumed to start at
-90 degrees, and occur at every 1 degree up to +90 degrees, with regard
to the robot's facing angle.*/
void outputLaser(FILE* fin, FILE* fout) {
//Expected values:
//-1.5708 +1.5708 +0.01745329 +5.6000 0181
double startangle, endangle, increment, minrange, maxrange;
unsigned int bins;
fscanf(fin, "%lf%lf%lf%lf%lf%u", &startangle, &endangle, &increment, &minrange, &maxrange, &bins);
//printf("%lf %lf %lf %lf %lf %u\n", startangle, endangle, increment, minrange, maxrange, bins);
if (bins != 181) {
fprintf(stderr, "Warning: Expected 181 bins, found %i; skipping line\n\n", bins);
skipLine(fin);
state = WAITING_LASER;
return;
}
fprintf(fout, "Laser 181 ");
int counter = 0;
char scanrange[128];
char intensity[128];
while (counter < 181) {
fscanf(fin, "%128s", scanrange);
fprintf(fout, "%s ", scanrange);
fscanf(fin, "%128s", intensity);
//Don't output the intensity (DP_SLAM doesn't want it)
counter++;
}
fprintf(fout, "\n");
fflush(fout);
}
void KmlGenerator::putStationStyle() {
startStyle("StationStyle");
putLabelStyle(ColorRGBA(0x00,0x00,0x00,0x00), NORMAL, 0.0f); //invisible labels
putIconStyle(stationIconHref, Offset(), 0.25f, 0.0f); //custom icon
endStyle();
skipLine();
}
void KmlGenerator::putIsoLines(const std::string &interpolatorName) {
const unsigned int nData = std::min(maxDataProcessed, sensorData.nMeasures);
const std::vector<IsoLineList<double,4u,float>> &interpIsolines = isolines.at(interpolatorName);
putFolder("Isolines", "Data isolines (" + interpolatorName + ")", false, true);
unsigned int j = 0;
for(const auto &temporalIsolines : interpIsolines) {
unsigned int i = 1;
for(const auto &isoline : temporalIsolines) {
putColorLineStrings("Isolines level " + std::to_string(i++),
"Isovalue: " + std::to_string(isoline.value) + " mg/m³",
"IsoLine_",
isoline.lines,
defaultVisibleInterpolatorId.compare(interpolatorName) == 0,
sensorData.getTime(j),
sensorData.getTime(j+1, true)
);
}
j++;
}
endFolder();
skipLine();
}
int Tokenizer::readInt(bool skipline) {
if (util::getNumberOfNumbersInString(m_sText) == 0)
roe_except("no ints found in '" + m_sText + "'", "readInt");
int i = util::parseInt(util::getFirstNumberOfStringAndErase(m_sText));
if (skipline) skipLine(1);
return i;
}
double Tokenizer::readDouble(bool skipline) {
if (util::getNumberOfNumbersInString(m_sText) == 0)
roe_except("no doubles found in '" + m_sText + "'", "readDouble");
double d = util::parseDouble(util::getFirstNumberOfStringAndErase(m_sText));
if (skipline) skipLine(1);
return d;
}
std::string Tokenizer::readString(bool skipline) {
if (util::getNumberOfStringsInString(m_sText, m_cStringDelim) == 0)
roe_except("no strings found in '" + m_sText + "'", "readString");
std::string s = util::getFirstStringOfStringAndErase(m_sText, m_cStringDelim);
if (skipline) skipLine(1);
return s;
}
bool StelJsonParserInstance::skipAndConsumeChar(char r)
{
char c;
while (getChar(&c))
{
switch (c)
{
case ' ':
case '\t':
case '\n':
case '\r':
break;
case '/':
{
if (!getChar(&c))
throw std::runtime_error(qPrintable(QString("Unexpected '/%1' in the JSON content").arg(c)));
if (c=='/')
skipLine();
else
{
// We have a problem, we removed a '/'.. This should never happen with properly formatted JSON though
throw std::runtime_error(qPrintable(QString("Unexpected '/%1' in the JSON content").arg(c)));
}
}
break;
default:
if (r==c)
return true;
ungetChar(c);
return false;
}
}
return false;
}
void StelJsonParserInstance::skipJson()
{
// There is a weakness in this code -- it will cause any standalone '/' to be absorbed.
char c;
while (getChar(&c))
{
switch (c)
{
case ' ':
case '\t':
case '\n':
case '\r':
break;
case '/':
{
if (!getChar(&c))
return;
if (c=='/')
skipLine();
else
{
// We have a problem, we removed a '/'.. This should never happen with properly formatted JSON though
throw std::runtime_error(qPrintable(QString("Unexpected '/%1' in the JSON content").arg(c)));
}
}
break;
default:
ungetChar(c);
return;
}
}
}
bool Tokenizer::readBool(bool skipline) {
if (util::getNumberOfBoolsInString(m_sText) == 0)
roe_except("no bool found in '" + m_sText + "'", "readBool");
bool b = util::getFirstBoolOfStringAndErase(m_sText);
if (skipline) skipLine(1);
return b;
}
void KmlGenerator::putIsoContours(const std::string &interpolatorName) {
const unsigned int nData = std::min(maxDataProcessed, sensorData.nMeasures);
const std::vector<IsoContourList<double,4u,float>> &interpIsocontours = isocontours.at(interpolatorName);
putFolder("Isocontours", "Data isocontours (" + interpolatorName + ")", false, false);
unsigned int j = 0;
for(const auto &temporalIsocontours : interpIsocontours) {
unsigned int i = 1;
for(auto isocontour = std::next(temporalIsocontours.begin()); isocontour != temporalIsocontours.end(); ++isocontour) {
auto prev = std::prev(isocontour);
putColorPolygons("Isocontour level " + std::to_string(i++),
"Min value: " + std::to_string(isocontour->lowerValue) + " mg/m³\n"
"Max value: " + std::to_string(isocontour->upperValue) + " mg/m³",
"IsoContour_",
ColorMultiLine<double,4u>(isocontour->lines, isocontour->color),
ColorMultiLine<double,4u>(prev->lines, prev->color),
false,
sensorData.getTime(j),
sensorData.getTime(j+1, true),
ABSOLUTE
);
}
j++;
}
endFolder();
skipLine();
}
void InConfig::skipWhitespace(PhysicalInStream& stream)
{
while(!isEof(stream) && isWhitespace())
{
while(!isEof(stream) && InText::isWhitespace())
nextChar(stream);
if(!isEof(stream))
{
if(theChar == '/' && theNextChar == '/')
skipLine(stream);
else if(theChar == '/' && theNextChar == '*')
skipComment(stream);
else if(theChar == '#')
skipLine(stream);
}
}
}
scene* sceneLoad()
{
int i, numSpheres = 0, numLights = 0, antiAliasing, resolution, recursion;
FILE *sceneFile = fopen("scene.dat", "r");
if (!sceneFile) {
printf("Scene file 'scene.dat' not found!");
return NULL;
}
scene *s = (scene*)malloc(sizeof(scene));
skipLine(sceneFile);
(void) fscanf(sceneFile, "%f %f %f", &s->bgcolor.x, &s->bgcolor.y, &s->bgcolor.z);
skipLine(sceneFile);
(void) fscanf(sceneFile, "%f %f %f", &s->camera.x, &s->camera.y, &s->camera.z);
skipLine(sceneFile);
(void) fscanf(sceneFile, "%d %d %d %d %d", &numSpheres, &numLights, &resolution, &antiAliasing, &recursion);
s->numSpheres = numSpheres + 0.5f;
s->numLights = numLights + 0.5f;
s->resolution = resolution + 0.5;
s->antiAliasing = antiAliasing + 0.5;
s->rayDepth = recursion + 0.5;
skipLine(sceneFile);
for (i = 0; i < numSpheres; i++)
{
(void) fscanf(sceneFile, "%f %f %f", &s->spheres[i].position.x, &s->spheres[i].position.y, &s->spheres[i].position.z);
(void) fscanf(sceneFile, "%f %f %f", &s->spheres[i].color.x, &s->spheres[i].color.y, &s->spheres[i].color.z);
(void) fscanf(sceneFile, "%f", &s->spheres[i].radius);
(void) fscanf(sceneFile, "%f", &s->spheres[i].reflection);
}
skipLine(sceneFile);
for (i = 0; i < numLights; i++)
{
(void) fscanf(sceneFile, "%f %f %f", &s->lights[i].position.x, &s->lights[i].position.y, &s->lights[i].position.z);
(void) fscanf(sceneFile, "%f %f %f", &s->lights[i].color.x, &s->lights[i].color.y, &s->lights[i].color.z);
}
fclose(sceneFile);
return s;
}
void KmlGenerator::putStations() {
putFolder("Stations", "Station locations", false, true);
for (unsigned int i = 0; i < sensorData.nStations; i++) {
putPlaceMark(*sensorData.stationNames[i], sensorData.stationDescription(i,getCurrentIndentLevel()+2), "StationStyle",
sensorData.x[i], sensorData.y[i], 0.0, CLAMP_TO_GROUND);
}
endFolder();
skipLine();
}
/* From DP_SLAM readme, odometry format needs to be:
ODOMETRY <x> <y> <theta>
The first argument denotes this as a reading from the robot's odometer.
<x> and <y> are the robot's current position from some arbitrary
starting point. These measures are in meters. <theta> is robot's
current facing angle, in radians.*/
void outputOdometry(FILE* fin, FILE* fout) {
//1245349407.171 16777343 6665 position2d 00 001 001 +00.439 -00.027 -0.101 +00.000 +00.000 +00.000 0
float x, y, theta;
fprintf(fout, "Odometry ");
fscanf(fin, "%f%f%f", &x, &y, &theta);
fprintf(fout, "%f %f %f \n", x, y, theta);
printf("%f %f %f \n", x, y, theta);
skipLine(fin);
fflush(fout);
}
void Tokenizer::readLine(std::string& s, bool skipline) {
std::string::size_type pos = m_sText.find("\n");
if (pos == std::string::npos) {
s = m_sText;
}
else {
s = m_sText.substr(0,pos);
}
if (skipline) skipLine(1);
}
void KmlGenerator::putKmlHeader() {
time_t t = time(0);
std::tm *now = localtime(&t);
startKml();
skipLine();
putComment("=====================================================================================================================");
putComment("This file was generated automatically with real meteorological data and is part of the Ensimag visualization project.");
putComment("=====================================================================================================================");
skipLine();
startDocument("Root");
putName("Environemental contaminant viewer");
putDescription("PM10 particles");
putDate(*now, YYYY_MM_DD_hh_mm_ss);
putAuthor("Jean-Baptiste Keck");
putAuthor("Alexandre Ribard");
skipLine();
putVisibility(true);
putOpen(true);
skipLine();
}
std::string Tokenizer::readLine(bool skipline) {
std::string::size_type pos = m_sText.find("\n");
std::string s;
if (pos == std::string::npos) {
s = m_sText;
}
else {
s = m_sText.substr(0,pos);
}
if (skipline) skipLine(1);
return s;
}
static void parse_DIMACS_main(B& in, Solver& S) {
vec<Lit> lits;
for (;;){
skipWhitespace(in);
if (*in == EOF)
break;
else if (*in == 'c' || *in == 'p')
skipLine(in);
else
readClause(in, S, lits),
S.addClause(lits);
}
}
void realNumberOfCPU(int* nRealCPU, int* nCPU)
{
char buffer [STAT_BUFSIZ];
char* p = buffer;
fileToBuffer(buffer, sizeof buffer, FILENAME);
// cpu
p = skipLine(p);
for(nRealCPU = 0; ; nRealCPU++)
{
if(!checkCPULine(p, nRealCPU))
{
nRealCPU--;
break;
}
p = skipLine(p);
}
nCPU = MIN(MONITA_NCPU - 1, nRealCPU);
}
int
readMMHeader(
SCOTCH_Num * vertnbr,
SCOTCH_Num * edgenbr,
FILE * const stream)
{
int c;
SCOTCH_Num vertnum0, vertnum1;
int symmetric = 0;
char firstline[1024];
int pos = 0;
memSet(firstline, '\0', 1024);
fgets(firstline, 16, stream);
if (strcmp(firstline, "%%MatrixMarket "))
return (-1);
while (((c = fgetc(stream)) != '\n') && (c != EOF)) {
firstline[pos++] = toupper(c);
if (pos == 1024)
return (-1);
}
firstline[pos] = '\0';
if (strstr(firstline, "SYMMETRIC"))
symmetric = 1;
while ((c = fgetc(stream)) == '%') {
if (skipLine(stream) != 0)
return (-1); /* End of file reached */
}
ungetc (c, stream);
if (intLoad (stream, &vertnum0) != 1) { /* Read row number */
return (-1);
}
if (intLoad (stream, &vertnum1) != 1) { /* Read col number */
return (-1);
}
if (vertnum0 != vertnum1) { /* Non square matrix */
return (-1);
}
*vertnbr = vertnum1;
if (intLoad (stream, edgenbr) != 1) { /* Read edge number */
return (-1);
}
*edgenbr -= *vertnbr; /* No loop in graph */
return (symmetric);
}
static void getCharacter(void)
{
/* Get the next character from the line buffer. If EOL, get next line */
inChar = *(FP->cp)++;
if (!inChar)
{
/* We have used all of the characters on this line. Read the next
* line of data
*/
skipLine();
}
}
void PlayerHeader::readHeader(FILE* fp) {
char buf[BUFSIZE];
fscanf(fp, "%128s", buf);
if (buf[0] == '#') {
// A comment is encountered, assume this is not a valid header
// Skip everything in a line that's started with a comment
skipLine(fp);
valid = false;
return;
}
valid = true;
time = atof(buf);
fscanf(fp, "%u%u%128s%u%u%u", &host, &robot, interfaceName, &index, &type, &subtype);
}
void outputOdometry(PlayerHeader h, FILE* fin, FILE* fout) {
//1245349407.171 fly 6665 position2d 00 001 001 +00.439 -00.027 -0.101 +00.000 +00.000 +00.000 0
//1053653857.290 fly 6665 position 00 1053652583.883 +01.200 -00.910 -1.501 -0.005 +0.000 +0.000
float x, y, theta;
float a, b, c, d;
fprintf(fout, "%lf %u %u %s %u %u %u ", h.time, h.host, h.robot, h.interfaceName, h.index, h.type, h.subtype);
fscanf(fin, "%f%f%f", &x, &y, &theta, &a, &b, &c, &d);
float newtheta = sqrt(abs(theta));
if (theta < 0) newtheta *= -1;
fprintf(fout, "%.2f %.2f %.2f %.2f %.2f %.2f %.2f\n", x, y, newtheta, a, b, c, d);
printf("%.2f\n", theta);
skipLine(fin);
fflush(fout);
}
void MeshManager::getVector2(std::vector<glm::vec2>* vertices, char* buffer, int* i){
getNextWord(buffer, i);
float x, y;
copyNextWord(tempBuffer, BUFFERSIZE, buffer, i);
x = (float) fast_atof(tempBuffer);
copyNextWord(tempBuffer, BUFFERSIZE, buffer, i);
y = (float) fast_atof(tempBuffer);
vertices->push_back(glm::vec2(x, y));
skipLine(buffer, i);
}
void
MIFrfile::skipImport()
{
int ch;
skipLine();
while ((ch = getChar()) != EOF) {
switch (ch) {
case '&':
case '=':
skipLine();
break;
case '\n':
case '\r':
case ' ':
break;
default:
ungetChar(ch);
return;
}
}
}
unsigned int hokuyo_readStatus(HokuyoURG* urg, char* cmd){
char buf[URG_BUFSIZE];
write (urg->fd, cmd, strlen(cmd));
while (1){
int c=hokuyo_readPacket(urg, buf, URG_BUFSIZE,10);
if (c>0 && !strncmp(buf,cmd+1,strlen(cmd)-1)){
char*s=buf;
s=skipLine(s);
char v[3]={s[0], s[1], 0};
return atoi(v);
}
}
return 0;
}
