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);
}
Esempio n. 2
0
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;
}