00001 #ifndef CAJUN_DISCONTINUITY_H
00002 #define CAJUN_DISCONTINUITY_H
00003
00004 #include "data_type.H"
00005 #include "data_queue.H"
00006 #include "data_logger.H"
00007 #include <cstdio>
00008
00009 # define SEARCH_DEPTH 225 //no of scans to be searched
00010 # define SEARCH_BUCKET 300
00011 # define MAX_SCANS 1000
00012 # define NUM_SENSORS 2
00013 # define SCANS_OVERWRITE (MAX_SCANS - SEARCH_BUCKET)
00014
00015
00016 namespace cajun
00017 {
00018
00019 # define scan_beam_p(sick_id,scan_num,beam_num) scan_repository[(sick_id)][(scan_num)].end_beam[(beam_num)]
00020
00021 # define assign_slope(curr_beam) assign_slope.end_beam[(curr_beam)]
00022 # define assign_slope_b(curr_beam) assign_slope_b.end_beam[(curr_beam)]
00023 class discontinuity_t
00024 {
00025 unsigned curr_scan[NUM_SENSORS];
00026 scan_analysis_data_t scan_repository[NUM_SENSORS][MAX_SCANS];
00027 data_queue_writer_t<scan_analysis_data_t> *m_dq_writer;
00028 data_queue_writer_t<scan_analysis_data_t> *m_dq_b_writer;
00029
00030 public:
00031 discontinuity_t (data_queue_writer_t<scan_analysis_data_t> *scan_analysis_dq_writer,data_queue_writer_t<scan_analysis_data_t> *scan_analysis_dq_b_writer)
00032 {
00033 for(int i=0; i< NUM_SENSORS; i++)
00034 curr_scan[i] = 0;
00035 m_dq_writer = scan_analysis_dq_writer;
00036 m_dq_b_writer = scan_analysis_dq_b_writer;
00037
00038 };
00039
00040 void add_scan (scan_gp_data_t const &points, unsigned sick_id, nav_data_t bot_posi)
00041 {
00042
00043 scan_repository[sick_id][curr_scan[sick_id]].num_beams = points.num_beams;
00044 scan_repository[sick_id][curr_scan[sick_id]].tstamp = points.tstamp;
00045 unsigned j;
00046 for (j = 0; j < scan_repository[sick_id][curr_scan[sick_id]].num_beams;j++)
00047 {
00048 scan_beam_p (sick_id,curr_scan[sick_id],j).x = points.end_beam[j].x;
00049 scan_beam_p (sick_id,curr_scan[sick_id],j).y = points.end_beam[j].y;
00050 scan_beam_p (sick_id,curr_scan[sick_id],j).z = points.end_beam[j].z;
00051 }
00052 find_discontinuity(scan_repository,curr_scan[sick_id],bot_posi,points,sick_id);
00053 curr_scan[sick_id]++;
00054 if (curr_scan[sick_id] > MAX_SCANS - 1)
00055 {
00056 for ( unsigned i = 0; i < SEARCH_BUCKET ; i++)
00057 {
00058 scan_repository[sick_id][i].num_beams = scan_repository[sick_id][i+SCANS_OVERWRITE].num_beams;
00059 scan_repository[sick_id][i].tstamp = scan_repository[sick_id][i+SCANS_OVERWRITE].tstamp;
00060
00061 for ( unsigned j = 0 ; j < scan_repository[sick_id][i].num_beams ; j++)
00062 {
00063 scan_beam_p (sick_id,i, j).x = scan_beam_p (sick_id,i+SCANS_OVERWRITE, j).x ;
00064 scan_beam_p (sick_id,i, j).y = scan_beam_p (sick_id,i+SCANS_OVERWRITE, j).y ;
00065 scan_beam_p (sick_id,i, j).z = scan_beam_p (sick_id,i+SCANS_OVERWRITE, j).z ;
00066 }
00067 }
00068 curr_scan[sick_id] = SEARCH_BUCKET;
00069 }
00070 }
00071
00072 void find_discontinuity (scan_analysis_data_t scan_repository[][MAX_SCANS], unsigned curr_scan, nav_data_t bot_posi, scan_gp_data_t points, unsigned sick_id);
00073
00074 };
00075 };
00076 #endif
