void SegmentStitching::runNode(){
std::vector<std::vector<Line> > segmentLines;
mapping_msgs::SegmentObjectVector allSegmentObjects;
// Go through all the segments, extract measurements, and convert these to lines.
for (size_t segment = 0; segment < mapSegments.size(); segment++) {
ROS_INFO("Processing segment %d of %d", (int)(segment + 1), (int)(mapSegments.size()));
// first, get the set of points which represent the measurements taken
// in the segment.
pcl::PointCloud<pcl::PointXYZ>::Ptr measurements(new pcl::PointCloud<pcl::PointXYZ>);
// the objects vector contains the relative positions and strings
// corresponding to the objects which were detected in the segment. Only
// one message is received when the object is first detected -
// subsequent detections do not add more information, so there is no
// need to do clustering to find the object position, though this might
// increase the accuracy.
mapping_msgs::ObjectVector objects;
segmentToMeasurements(mapSegments[segment], measurements, objects);
// Extract the lines from this segment using ransac. This operation
// destroys the measurement pointcloud
std::vector<Line> lines = extractLinesFromMeasurements(measurements, ransacThreshold);
segmentLines.push_back(lines);
allSegmentObjects.segmentObjects.push_back(objects);
// tmpPublish(measurements, lines);
}
std::vector<std::vector<Line> > stitchedLines = processSegments(segmentLines, allSegmentObjects);
publishFinalMessages(stitchedLines, allSegmentObjects);
// publishSegmentLines(stitchedLines);
}
int main(int argc, char *argv[])
{
int nn,qq;
static char fname[500];
char *p;
ofp = stdout;
nn = processOptions(argc, argv);
if (nn > argc-1) {
displayHelp();
return 0;
}
strcpy(fname, argv[nn]);
mfndx = 0;
start_address = 0;
code_address = 0;
bss_address = 0;
for (qq = 0; qq < 12; qq++)
sections[qq].Clear();
nmTable.Clear();
memset(masterFile,0,sizeof(masterFile));
if (verbose) printf("Pass 1 - collect all input files.\r\n");
processFile(fname,0); // Pass 1, collect all include files
if (debug) {
FILE *fp;
fp = fopen("a64-master.asm", "w");
if (fp) {
fwrite(masterFile, 1, strlen(masterFile), fp);
fclose(fp);
}
}
if (verbose) printf("Pass 2 - group and reorder segments\r\n");
processSegments(); // Pass 2, group and order segments
pass = 3;
if (verbose) printf("Pass 3 - get all symbols, set initial values.\r\n");
processMaster();
pass = 4;
phasing_errors = 0;
if (verbose) printf("Pass 4 - assemble code.\r\n");
processMaster();
if (verbose) printf("Pass 4: phase errors: %d\r\n", phasing_errors);
pass = 5;
while (phasing_errors && pass < 10) {
phasing_errors = 0;
processMaster();
if (verbose) printf("Pass %d: phase errors: %d\r\n", pass, phasing_errors);
pass++;
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Output listing file.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ofp = (FILE *)NULL;
if (listing) {
if (verbose) printf("Generating listing file.\r\n");
strcpy(fname, argv[nn]);
p = strrchr(fname,'.');
if (p) {
*p = '\0';
}
strcat(fname, ".lst");
ofp = fopen(fname,"w");
if (!ofp)
printf("Can't open output file <%s>\r\n", fname);
bGen = 1;
}
processMaster();
DumpSymbols();
if (listing)
fclose(ofp);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Output binary file.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (binary_out) {
if (verbose) printf("Generating binary file.\r\n");
strcpy(fname, argv[nn]);
p = strrchr(fname,'.');
if (p) {
*p = '\0';
}
strcat(fname, ".bin");
ofp = fopen(fname,"wb");
if (ofp) {
fwrite((void*)sections[0].bytes,sections[0].index,1,ofp);
fwrite((void*)sections[1].bytes,sections[1].index,1,ofp);
fwrite((void*)sections[2].bytes,sections[2].index,1,ofp);
//fwrite(binfile,binndx,1,ofp);
fclose(ofp);
}
else
printf("Can't create .bin file.\r\n");
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Output ELF file.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (elf_out) {
if (verbose) printf("Generating ELF file.\r\n");
strcpy(fname, argv[nn]);
p = strrchr(fname,'.');
if (p) {
*p = '\0';
}
if (rel_out)
strcat(fname, ".rel");
else
strcat(fname, ".elf");
ofp = fopen(fname,"wb");
if (ofp) {
WriteELFFile(ofp);
fclose(ofp);
}
else
printf("Can't create .elf file.\r\n");
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Output Verilog memory declaration
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if (verilog_out) {
if (verbose) printf("Generating Verilog file.\r\n");
strcpy(fname, argv[nn]);
p = strrchr(fname,'.');
if (p) {
*p = '\0';
}
strcat(fname, ".ver");
vfp = fopen(fname, "w");
if (vfp) {
if (gCpu==64) {
for (nn = 0; nn < binndx; nn+=8) {
fprintf(vfp, "\trommem[%d] = 65'h%01d%02X%02X%02X%02X%02X%02X%02X%02X;\n",
(((start_address+nn)/8)%8192), checksum64((int64_t *)&binfile[nn]),
binfile[nn+7], binfile[nn+6], binfile[nn+5], binfile[nn+4],
binfile[nn+3], binfile[nn+2], binfile[nn+1], binfile[nn]);
}
}
else {
for (nn = 0; nn < binndx; nn+=4) {
fprintf(vfp, "\trommem[%d] = 33'h%01d%02X%02X%02X%02X;\n",
(((start_address+nn)/4)%8192), checksum((int32_t *)&binfile[nn]), binfile[nn+3], binfile[nn+2], binfile[nn+1], binfile[nn]);
}
}
fclose(vfp);
}
else
printf("Can't create .ver file.\r\n");
}
return 0;
}
