dummy_obstacles.H

Go to the documentation of this file.

#ifndef CAJUN_DUMMY_SLOPE_H
#define CAJUN_DUMMY_SLOPE_H

#include "data_type.H"
#include "data_queue.H"
#include "data_logger.H"
#include <cstdio>

namespace cajun
{
# define scan_beam(scan_num, beam_num) scan_repository[(scan_num)].end_beam[(beam_num)]
# define assign_slope(curr_beam) assign_slope.end_beam[(curr_beam)]
# define MAX_OBSTACLES 2000

class dummy_obstacles_t
    {
        data_queue_writer_t<scan_analysis_data_t> *m_dq_b_writer;
        data_queue_writer_t<scan_analysis_data_t> *m_dq_writer;
        struct obst_posi
        {
                double X;
                double Y;
        };
        obst_posi obst_details[MAX_OBSTACLES];
        int num_obst;
            FILE *fp;
    public:
        dummy_obstacles_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,
                           const char *obstacle_source_filename)
            {
                m_dq_writer = scan_analysis_dq_writer;
                m_dq_b_writer = scan_analysis_dq_b_writer;
                fp = fopen(obstacle_source_filename,"r");
                fscanf(fp,"%d",&num_obst);
                for(int i = 0 ; i < num_obst ; i++)
                {
                        fscanf(fp,"%lf %lf",&obst_details[i].X,&obst_details[i].Y);
                }

            };

            void add_scan (nav_data_t nav)
                        {
                                scan_analysis_data_t assign_slope;
                                assign_slope.tstamp = nav.tstamp;
                                assign_slope.num_beams = num_obst;
                                for ( int i = 0; i < num_obst; i++)
                                {
                                        assign_slope(i).x = 0.0;
                                        assign_slope(i).y = 0.0;
                                        assign_slope(i).z = 0.0;
                                        assign_slope(i).pitch_slope = 0.0;
                                        assign_slope(i).roll_slope = 0.0;
                                }
                                for ( int i = 0;i < num_obst; i++)
                                {
                                        assign_slope(i).x = obst_details[i].X;
                                        assign_slope(i).y = obst_details[i].Y;
                                        assign_slope(i).z = 0;
                                        assign_slope(i).pitch_slope = 90.0;
                                        assign_slope(i).roll_slope = 90.0;
                                }

                                m_dq_b_writer->push(assign_slope);

                        }

            };
};
#endif

---