world_state.H

Go to the documentation of this file.

// Copyright (C) 2006 University of Louisiana at Lafayette
// Authors: Suresh Golconda

#ifndef CAJUN_WORLD_STATE_H
#define CAJUN_WORLD_STATE_H

#include "access_data.H"
#include "cb_time.H"
#include <cassert>

#include "rndf_lane_container.H"
#include "blocked_lane_container.H"
#include "radar_data_collector.H"
#include "intersection_tracker.H"
#include "tstamp_buffering.H"
#include "spatial_to_lane_map.H"

#include "rndf.H"

namespace cajun
{

class func_t
{
public:
        virtual bool operator() () = 0;
        virtual ~func_t () = 0;
};

inline func_t::~func_t () {}

class world_state_t
{
public:
        world_state_t (access_data_t *ad_, rndf_t *rndf = NULL);
        ~world_state_t ();
private:
        access_data_t *m_ad;
        acceleration_data_t m_acceleration[1];

        class func_acceleration_t : public func_t
        {
        public:
                virtual ~func_acceleration_t () { }
                func_acceleration_t (acceleration_data_t *data_, int qid_, access_data_t *ad_)
                        : m_acceleration (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_acceleration (*m_acceleration, m_qid);
                }
        private:
                acceleration_data_t *m_acceleration;
                int m_qid;
                access_data_t *m_ad;
        };
        angular_rate_data_t m_angular_rate[1];

        class func_angular_rate_t : public func_t
        {
        public:
                virtual ~func_angular_rate_t () { }
                func_angular_rate_t (angular_rate_data_t *data_, int qid_, access_data_t *ad_)
                        : m_angular_rate (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_angular_rate (*m_angular_rate, m_qid);
                }
        private:
                angular_rate_data_t *m_angular_rate;
                int m_qid;
                access_data_t *m_ad;
        };
        cbln_data_t m_cbln[1];

        class func_cbln_t : public func_t
        {
        public:
                virtual ~func_cbln_t () { }
                func_cbln_t (cbln_data_t *data_, int qid_, access_data_t *ad_)
                        : m_cbln (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_cbln (*m_cbln, m_qid);
                }
        private:
                cbln_data_t *m_cbln;
                int m_qid;
                access_data_t *m_ad;
        };
        cbln_dtm_data_t m_cbln_dtm[1];

        class func_cbln_dtm_t : public func_t
        {
        public:
                virtual ~func_cbln_dtm_t ()     { }
                func_cbln_dtm_t (cbln_dtm_data_t *data_, int qid_, access_data_t *ad_)
                        : m_cbln_dtm (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_cbln_dtm (*m_cbln_dtm, m_qid);
                }
        private:
                cbln_dtm_data_t *m_cbln_dtm;
                int m_qid;
                access_data_t *m_ad;
        };
        cbmesg_data_t m_cbmesg[1];

        class func_cbmesg_t : public func_t
        {
        public:
                virtual ~func_cbmesg_t ()       { }
                func_cbmesg_t (cbmesg_data_t *data_, int qid_, access_data_t *ad_)
                        : m_cbmesg (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_cbmesg (*m_cbmesg, m_qid);
                }
        private:
                cbmesg_data_t *m_cbmesg;
                int m_qid;
                access_data_t *m_ad;
        };
        cog_data_t m_cog[1];

        class func_cog_t : public func_t
        {
        public:
                virtual ~func_cog_t ()  { }
                func_cog_t (cog_data_t *data_, int qid_, access_data_t *ad_)
                        : m_cog (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_cog (*m_cog, m_qid);
                }
        private:
                cog_data_t *m_cog;
                int m_qid;
                access_data_t *m_ad;
        };
        control_data_t m_control[1];

        class func_control_t : public func_t
        {
        public:
                virtual ~func_control_t ()      { }
                func_control_t (control_data_t *data_, int qid_, access_data_t *ad_)
                        : m_control (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_control (*m_control, m_qid);
                }
        private:
                control_data_t *m_control;
                int m_qid;
                access_data_t *m_ad;
        };
        control_line_data_t m_control_line[1];

        class func_control_line_t : public func_t
        {
        public:
                virtual ~func_control_line_t () { }
                func_control_line_t (control_line_data_t *data_, int qid_, access_data_t *ad_)
                        : m_control_line (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_control_line (*m_control_line, m_qid);
                }
        private:
                control_line_data_t *m_control_line;
                int m_qid;
                access_data_t *m_ad;
        };
        drive_data_t m_drive[2];

        class func_drive_t : public func_t
        {
        public:
                virtual ~func_drive_t ()        { }
                func_drive_t (drive_data_t *data_, int qid_, access_data_t *ad_)
                        : m_drive (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_drive (*m_drive, m_qid);
                }
        private:
                drive_data_t *m_drive;
                int m_qid;
                access_data_t *m_ad;
        };
        grid_path_data_t m_grid_path[1];

        class func_grid_path_t : public func_t
        {
        public:
                virtual ~func_grid_path_t ()    { }
                func_grid_path_t (grid_path_data_t *data_, int qid_, access_data_t *ad_)
                        : m_grid_path (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_grid_path (*m_grid_path, m_qid);
                }
        private:
                grid_path_data_t *m_grid_path;
                int m_qid;
                access_data_t *m_ad;
        };
        grid_tile_data_t m_grid_tile[2];

        class func_grid_tile_t : public func_t
        {
        public:
                virtual ~func_grid_tile_t ()    { }
                func_grid_tile_t (grid_tile_data_t *data_, int qid_, access_data_t *ad_)
                        : m_grid_tile (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_grid_tile (*m_grid_tile, m_qid);
                }
        private:
                grid_tile_data_t *m_grid_tile;
                int m_qid;
                access_data_t *m_ad;
        };
        ground_wheel_angle_data_t m_ground_wheel_angle[1];

        class func_ground_wheel_angle_t : public func_t
        {
        public:
                virtual ~func_ground_wheel_angle_t ()   { }
                func_ground_wheel_angle_t (ground_wheel_angle_data_t *data_, int qid_, access_data_t *ad_)
                        : m_ground_wheel_angle (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_ground_wheel_angle (*m_ground_wheel_angle, m_qid);
                }
        private:
                ground_wheel_angle_data_t *m_ground_wheel_angle;
                int m_qid;
                access_data_t *m_ad;
        };
        guide_data_t m_guide[1];

        class func_guide_t : public func_t
        {
        public:
                virtual ~func_guide_t ()        { }
                func_guide_t (guide_data_t *data_, int qid_, access_data_t *ad_)
                        : m_guide (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_guide (*m_guide, m_qid);
                }
        private:
                guide_data_t *m_guide;
                int m_qid;
                access_data_t *m_ad;
        };
        heading_data_t m_heading[1];

        class func_heading_t : public func_t
        {
        public:
                virtual ~func_heading_t ()      { }
                func_heading_t (heading_data_t *data_, int qid_, access_data_t *ad_)
                        : m_heading (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_heading (*m_heading, m_qid);
                }
        private:
                heading_data_t *m_heading;
                int m_qid;
                access_data_t *m_ad;
        };
        ibeo_object_data_t m_ibeo_object[2];

        class func_ibeo_object_t : public func_t
        {
        public:
                virtual ~func_ibeo_object_t ()  { }
                func_ibeo_object_t (ibeo_object_data_t *data_, int qid_, access_data_t *ad_)
                        : m_ibeo_object (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_ibeo_object (*m_ibeo_object, m_qid);
                }
        private:
                ibeo_object_data_t *m_ibeo_object;
                int m_qid;
                access_data_t *m_ad;
        };
        ibeo_scan_data_t m_ibeo_scan[1];

        class func_ibeo_scan_t : public func_t
        {
        public:
                virtual ~func_ibeo_scan_t ()    { }
                func_ibeo_scan_t (ibeo_scan_data_t *data_, int qid_, access_data_t *ad_)
                        : m_ibeo_scan (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_ibeo_scan (*m_ibeo_scan, m_qid);
                }
        private:
                ibeo_scan_data_t *m_ibeo_scan;
                int m_qid;
                access_data_t *m_ad;
        };
        image_data_t m_image[1];

        class func_image_t : public func_t
        {
        public:
                virtual ~func_image_t ()        { }
                func_image_t (image_data_t *data_, int qid_, access_data_t *ad_)
                        : m_image (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_image (*m_image, m_qid);
                }
        private:
                image_data_t *m_image;
                int m_qid;
                access_data_t *m_ad;
        };
        iteris_lane_data_t m_iteris_lane[1];

        class func_iteris_lane_t : public func_t
        {
        public:
                virtual ~func_iteris_lane_t ()  { }
                func_iteris_lane_t (iteris_lane_data_t *data_, int qid_, access_data_t *ad_)
                        : m_iteris_lane (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_iteris_lane (*m_iteris_lane, m_qid);
                }
        private:
                iteris_lane_data_t *m_iteris_lane;
                int m_qid;
                access_data_t *m_ad;
        };
        iteris_lane_gp_data_t m_iteris_lane_gp[1];

        class func_iteris_lane_gp_t : public func_t
        {
        public:
                virtual ~func_iteris_lane_gp_t ()       { }
                func_iteris_lane_gp_t (iteris_lane_gp_data_t *data_, int qid_, access_data_t *ad_)
                        : m_iteris_lane_gp (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_iteris_lane_gp (*m_iteris_lane_gp, m_qid);
                }
        private:
                iteris_lane_gp_data_t *m_iteris_lane_gp;
                int m_qid;
                access_data_t *m_ad;
        };
        lane_data_t m_lane[1];

        class func_lane_t : public func_t
        {
        public:
                virtual ~func_lane_t () { }
                func_lane_t (lane_data_t *data_, int qid_, access_data_t *ad_)
                        : m_lane (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_lane (*m_lane, m_qid);
                }
        private:
                lane_data_t *m_lane;
                int m_qid;
                access_data_t *m_ad;
        };
        local_dtm_data_t m_local_dtm[2];

        class func_local_dtm_t : public func_t
        {
        public:
                virtual ~func_local_dtm_t ()    { }
                func_local_dtm_t (local_dtm_data_t *data_, int qid_, access_data_t *ad_)
                        : m_local_dtm (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_local_dtm (*m_local_dtm, m_qid);
                }
        private:
                local_dtm_data_t *m_local_dtm;
                int m_qid;
                access_data_t *m_ad;
        };
        mesh_cell_data_t m_mesh_cell[4];

        class func_mesh_cell_t : public func_t
        {
        public:
                virtual ~func_mesh_cell_t ()    { }
                func_mesh_cell_t (mesh_cell_data_t *data_, int qid_, access_data_t *ad_)
                        : m_mesh_cell (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_mesh_cell (*m_mesh_cell, m_qid);
                }
        private:
                mesh_cell_data_t *m_mesh_cell;
                int m_qid;
                access_data_t *m_ad;
        };
        mission_status_data_t m_mission_status[1];

        class func_mission_status_t : public func_t
        {
        public:
                virtual ~func_mission_status_t ()       { }
                func_mission_status_t (mission_status_data_t *data_, int qid_, access_data_t *ad_)
                        : m_mission_status (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_mission_status (*m_mission_status, m_qid);
                }
        private:
                mission_status_data_t *m_mission_status;
                int m_qid;
                access_data_t *m_ad;
        };
        motion_data_t m_motion[1];

        class func_motion_t : public func_t
        {
        public:
                virtual ~func_motion_t ()       { }
                func_motion_t (motion_data_t *data_, int qid_, access_data_t *ad_)
                        : m_motion (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_motion (*m_motion, m_qid);
                }
        private:
                motion_data_t *m_motion;
                int m_qid;
                access_data_t *m_ad;
        };
        nav_data_t m_nav[2];

        class func_nav_t : public func_t
        {
        public:
                virtual ~func_nav_t ()  { }
                func_nav_t (nav_data_t *data_, int qid_, access_data_t *ad_)
                        : m_nav (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_nav (*m_nav, m_qid);
                }
        private:
                nav_data_t *m_nav;
                int m_qid;
                access_data_t *m_ad;
        };
        path_data_t m_path[3];

        class func_path_t : public func_t
        {
        public:
                virtual ~func_path_t () { }
                func_path_t (path_data_t *data_, int qid_, access_data_t *ad_)
                        : m_path (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_path (*m_path, m_qid);
                }
        private:
                path_data_t *m_path;
                int m_qid;
                access_data_t *m_ad;
        };
        plan_status_data_t m_plan_status[1];

        class func_plan_status_t : public func_t
        {
        public:
                virtual ~func_plan_status_t ()  { }
                func_plan_status_t (plan_status_data_t *data_, int qid_, access_data_t *ad_)
                        : m_plan_status (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_plan_status (*m_plan_status, m_qid);
                }
        private:
                plan_status_data_t *m_plan_status;
                int m_qid;
                access_data_t *m_ad;
        };
        playback_control_data_t m_playback_control[1];

        class func_playback_control_t : public func_t
        {
        public:
                virtual ~func_playback_control_t ()     { }
                func_playback_control_t (playback_control_data_t *data_, int qid_, access_data_t *ad_)
                        : m_playback_control (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_playback_control (*m_playback_control, m_qid);
                }
        private:
                playback_control_data_t *m_playback_control;
                int m_qid;
                access_data_t *m_ad;
        };
        radar_data_t m_radar[4];

        class func_radar_t : public func_t
        {
        public:
                virtual ~func_radar_t ()        { }
                func_radar_t (radar_data_t *data_, int qid_, access_data_t *ad_)
                        : m_radar (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_radar (*m_radar, m_qid);
                }
        private:
                radar_data_t *m_radar;
                int m_qid;
                access_data_t *m_ad;
        };
        radar_gp_data_t m_radar_gp[4];

        class func_radar_gp_t : public func_t
        {
        public:
                virtual ~func_radar_gp_t ()     { }
                func_radar_gp_t (radar_gp_data_t *data_, int qid_, access_data_t *ad_)
                        : m_radar_gp (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_radar_gp (*m_radar_gp, m_qid);
                }
        private:
                radar_gp_data_t *m_radar_gp;
                int m_qid;
                access_data_t *m_ad;
        };
        range_data_t m_range[3];

        class func_range_t : public func_t
        {
        public:
                virtual ~func_range_t ()        { }
                func_range_t (range_data_t *data_, int qid_, access_data_t *ad_)
                        : m_range (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_range (*m_range, m_qid);
                }
        private:
                range_data_t *m_range;
                int m_qid;
                access_data_t *m_ad;
        };
        scan_analysis_data_t m_scan_analysis[2];

        class func_scan_analysis_t : public func_t
        {
        public:
                virtual ~func_scan_analysis_t ()        { }
                func_scan_analysis_t (scan_analysis_data_t *data_, int qid_, access_data_t *ad_)
                        : m_scan_analysis (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_scan_analysis (*m_scan_analysis, m_qid);
                }
        private:
                scan_analysis_data_t *m_scan_analysis;
                int m_qid;
                access_data_t *m_ad;
        };
        scan_data_t m_scan[8];

        class func_scan_t : public func_t
        {
        public:
                virtual ~func_scan_t () { }
                func_scan_t (scan_data_t *data_, int qid_, access_data_t *ad_)
                        : m_scan (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_scan (*m_scan, m_qid);
                }
        private:
                scan_data_t *m_scan;
                int m_qid;
                access_data_t *m_ad;
        };
        scan_dxyz_data_t m_scan_dxyz[2];

        class func_scan_dxyz_t : public func_t
        {
        public:
                virtual ~func_scan_dxyz_t ()    { }
                func_scan_dxyz_t (scan_dxyz_data_t *data_, int qid_, access_data_t *ad_)
                        : m_scan_dxyz (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_scan_dxyz (*m_scan_dxyz, m_qid);
                }
        private:
                scan_dxyz_data_t *m_scan_dxyz;
                int m_qid;
                access_data_t *m_ad;
        };
        scan_gp_data_t m_scan_gp[8];

        class func_scan_gp_t : public func_t
        {
        public:
                virtual ~func_scan_gp_t ()      { }
                func_scan_gp_t (scan_gp_data_t *data_, int qid_, access_data_t *ad_)
                        : m_scan_gp (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_scan_gp (*m_scan_gp, m_qid);
                }
        private:
                scan_gp_data_t *m_scan_gp;
                int m_qid;
                access_data_t *m_ad;
        };
        scan_lane_gp_data_t m_scan_lane_gp[1];

        class func_scan_lane_gp_t : public func_t
        {
        public:
                virtual ~func_scan_lane_gp_t () { }
                func_scan_lane_gp_t (scan_lane_gp_data_t *data_, int qid_, access_data_t *ad_)
                        : m_scan_lane_gp (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_scan_lane_gp (*m_scan_lane_gp, m_qid);
                }
        private:
                scan_lane_gp_data_t *m_scan_lane_gp;
                int m_qid;
                access_data_t *m_ad;
        };
        sensor_position_data_t m_sensor_position[1];

        class func_sensor_position_t : public func_t
        {
        public:
                virtual ~func_sensor_position_t ()      { }
                func_sensor_position_t (sensor_position_data_t *data_, int qid_, access_data_t *ad_)
                        : m_sensor_position (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_sensor_position (*m_sensor_position, m_qid);
                }
        private:
                sensor_position_data_t *m_sensor_position;
                int m_qid;
                access_data_t *m_ad;
        };
        signal_line_data_t m_signal_line[1];

        class func_signal_line_t : public func_t
        {
        public:
                virtual ~func_signal_line_t ()  { }
                func_signal_line_t (signal_line_data_t *data_, int qid_, access_data_t *ad_)
                        : m_signal_line (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_signal_line (*m_signal_line, m_qid);
                }
        private:
                signal_line_data_t *m_signal_line;
                int m_qid;
                access_data_t *m_ad;
        };
        sim_time_data_t m_sim_time[1];

        class func_sim_time_t : public func_t
        {
        public:
                virtual ~func_sim_time_t ()     { }
                func_sim_time_t (sim_time_data_t *data_, int qid_, access_data_t *ad_)
                        : m_sim_time (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_sim_time (*m_sim_time, m_qid);
                }
        private:
                sim_time_data_t *m_sim_time;
                int m_qid;
                access_data_t *m_ad;
        };
        sog_data_t m_sog[1];

        class func_sog_t : public func_t
        {
        public:
                virtual ~func_sog_t ()  { }
                func_sog_t (sog_data_t *data_, int qid_, access_data_t *ad_)
                        : m_sog (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_sog (*m_sog, m_qid);
                }
        private:
                sog_data_t *m_sog;
                int m_qid;
                access_data_t *m_ad;
        };
        steering_data_t m_steering[1];

        class func_steering_t : public func_t
        {
        public:
                virtual ~func_steering_t ()     { }
                func_steering_t (steering_data_t *data_, int qid_, access_data_t *ad_)
                        : m_steering (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_steering (*m_steering, m_qid);
                }
        private:
                steering_data_t *m_steering;
                int m_qid;
                access_data_t *m_ad;
        };
        survey_gp_data_t m_survey_gp[2];

        class func_survey_gp_t : public func_t
        {
        public:
                virtual ~func_survey_gp_t ()    { }
                func_survey_gp_t (survey_gp_data_t *data_, int qid_, access_data_t *ad_)
                        : m_survey_gp (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_survey_gp (*m_survey_gp, m_qid);
                }
        private:
                survey_gp_data_t *m_survey_gp;
                int m_qid;
                access_data_t *m_ad;
        };
        triangle_data_t m_triangle_data[4];

        class func_triangle_data_t : public func_t
        {
        public:
                virtual ~func_triangle_data_t ()        { }
                func_triangle_data_t (triangle_data_t *data_, int qid_, access_data_t *ad_)
                        : m_triangle_data (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_triangle_data (*m_triangle_data, m_qid);
                }
        private:
                triangle_data_t *m_triangle_data;
                int m_qid;
                access_data_t *m_ad;
        };
        vehicle_data_t m_vehicle[1];

        class func_vehicle_t : public func_t
        {
        public:
                virtual ~func_vehicle_t ()      { }
                func_vehicle_t (vehicle_data_t *data_, int qid_, access_data_t *ad_)
                        : m_vehicle (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_vehicle (*m_vehicle, m_qid);
                }
        private:
                vehicle_data_t *m_vehicle;
                int m_qid;
                access_data_t *m_ad;
        };
        vpath_data_t m_vpath[1];

        class func_vpath_t : public func_t
        {
        public:
                virtual ~func_vpath_t ()        { }
                func_vpath_t (vpath_data_t *data_, int qid_, access_data_t *ad_)
                        : m_vpath (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_vpath (*m_vpath, m_qid);
                }
        private:
                vpath_data_t *m_vpath;
                int m_qid;
                access_data_t *m_ad;
        };
        waypt_data_t m_waypt[1];

        class func_waypt_t : public func_t
        {
        public:
                virtual ~func_waypt_t ()        { }
                func_waypt_t (waypt_data_t *data_, int qid_, access_data_t *ad_)
                        : m_waypt (data_), m_qid (qid_), m_ad (ad_) { }
                bool operator() ()
                {
                        return m_ad->get_waypt (*m_waypt, m_qid);
                }
        private:
                waypt_data_t *m_waypt;
                int m_qid;
                access_data_t *m_ad;
        };

        std::vector<func_t *> m_func_vector;
        func_t *func;
        unsigned m_rndf_lane_qid;
public:
        //return copy of access_data pointer
        access_data_t *get_access_data () const
        {
                return m_ad;
        }

        // *********** acceleration *************
        void enable_acceleration_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_acceleration_read (qid))
                {
                        func = new func_acceleration_t (&m_acceleration[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_acceleration_read (qid);
                }
        }

        bool get_acceleration (acceleration_data_t &d_, int qid)
        {
                if (m_ad && m_ad->acceleration_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_acceleration[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_acceleration_write (int qid)
        { if (m_ad)
                m_ad->enable_acceleration_write(qid);
        }

        bool publish_acceleration(acceleration_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_acceleration_write (qid))
                return m_ad->publish_acceleration (d_, qid);
           else
                return false;
        }
        // *********** angular_rate *************
        void enable_angular_rate_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_angular_rate_read (qid))
                {
                        func = new func_angular_rate_t (&m_angular_rate[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_angular_rate_read (qid);
                }
        }

        bool get_angular_rate (angular_rate_data_t &d_, int qid)
        {
                if (m_ad && m_ad->angular_rate_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_angular_rate[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_angular_rate_write (int qid)
        { if (m_ad)
                m_ad->enable_angular_rate_write(qid);
        }

        bool publish_angular_rate(angular_rate_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_angular_rate_write (qid))
                return m_ad->publish_angular_rate (d_, qid);
           else
                return false;
        }
        // *********** cbln *************
        void enable_cbln_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_cbln_read (qid))
                {
                        func = new func_cbln_t (&m_cbln[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_cbln_read (qid);
                }
        }

        bool get_cbln (cbln_data_t &d_, int qid)
        {
                if (m_ad && m_ad->cbln_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_cbln[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_cbln_write (int qid)
        { if (m_ad)
                m_ad->enable_cbln_write(qid);
        }

        bool publish_cbln(cbln_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_cbln_write (qid))
                return m_ad->publish_cbln (d_, qid);
           else
                return false;
        }
        // *********** cbln_dtm *************
        void enable_cbln_dtm_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_cbln_dtm_read (qid))
                {
                        func = new func_cbln_dtm_t (&m_cbln_dtm[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_cbln_dtm_read (qid);
                }
        }

        bool get_cbln_dtm (cbln_dtm_data_t &d_, int qid)
        {
                if (m_ad && m_ad->cbln_dtm_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_cbln_dtm[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_cbln_dtm_write (int qid)
        { if (m_ad)
                m_ad->enable_cbln_dtm_write(qid);
        }

        bool publish_cbln_dtm(cbln_dtm_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_cbln_dtm_write (qid))
                return m_ad->publish_cbln_dtm (d_, qid);
           else
                return false;
        }
        // *********** cbmesg *************
        void enable_cbmesg_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_cbmesg_read (qid))
                {
                        func = new func_cbmesg_t (&m_cbmesg[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_cbmesg_read (qid);
                }
        }

        bool get_cbmesg (cbmesg_data_t &d_, int qid)
        {
                if (m_ad && m_ad->cbmesg_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_cbmesg[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_cbmesg_write (int qid)
        { if (m_ad)
                m_ad->enable_cbmesg_write(qid);
        }

        bool publish_cbmesg(cbmesg_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_cbmesg_write (qid))
                return m_ad->publish_cbmesg (d_, qid);
           else
                return false;
        }
        // *********** cog *************
        void enable_cog_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_cog_read (qid))
                {
                        func = new func_cog_t (&m_cog[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_cog_read (qid);
                }
        }

        bool get_cog (cog_data_t &d_, int qid)
        {
                if (m_ad && m_ad->cog_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_cog[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_cog_write (int qid)
        { if (m_ad)
                m_ad->enable_cog_write(qid);
        }

        bool publish_cog(cog_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_cog_write (qid))
                return m_ad->publish_cog (d_, qid);
           else
                return false;
        }
        // *********** control *************
        void enable_control_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_control_read (qid))
                {
                        func = new func_control_t (&m_control[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_control_read (qid);
                }
        }

        bool get_control (control_data_t &d_, int qid)
        {
                if (m_ad && m_ad->control_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_control[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_control_write (int qid)
        { if (m_ad)
                m_ad->enable_control_write(qid);
        }

        bool publish_control(control_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_control_write (qid))
                return m_ad->publish_control (d_, qid);
           else
                return false;
        }
        // *********** control_line *************
        void enable_control_line_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_control_line_read (qid))
                {
                        func = new func_control_line_t (&m_control_line[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_control_line_read (qid);
                }
        }

        bool get_control_line (control_line_data_t &d_, int qid)
        {
                if (m_ad && m_ad->control_line_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_control_line[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_control_line_write (int qid)
        { if (m_ad)
                m_ad->enable_control_line_write(qid);
        }

        bool publish_control_line(control_line_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_control_line_write (qid))
                return m_ad->publish_control_line (d_, qid);
           else
                return false;
        }
        // *********** drive *************
        void enable_drive_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_drive_read (qid))
                {
                        func = new func_drive_t (&m_drive[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_drive_read (qid);
                }
        }

        bool get_drive (drive_data_t &d_, int qid)
        {
                if (m_ad && m_ad->drive_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_drive[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_drive_write (int qid)
        { if (m_ad)
                m_ad->enable_drive_write(qid);
        }

        bool publish_drive(drive_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_drive_write (qid))
                return m_ad->publish_drive (d_, qid);
           else
                return false;
        }
        // *********** grid_path *************
        void enable_grid_path_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_grid_path_read (qid))
                {
                        func = new func_grid_path_t (&m_grid_path[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_grid_path_read (qid);
                }
        }

        bool get_grid_path (grid_path_data_t &d_, int qid)
        {
                if (m_ad && m_ad->grid_path_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_grid_path[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_grid_path_write (int qid)
        { if (m_ad)
                m_ad->enable_grid_path_write(qid);
        }

        bool publish_grid_path(grid_path_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_grid_path_write (qid))
                return m_ad->publish_grid_path (d_, qid);
           else
                return false;
        }
        // *********** grid_tile *************
        void enable_grid_tile_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_grid_tile_read (qid))
                {
                        func = new func_grid_tile_t (&m_grid_tile[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_grid_tile_read (qid);
                }
        }

        bool get_grid_tile (grid_tile_data_t &d_, int qid)
        {
                if (m_ad && m_ad->grid_tile_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_grid_tile[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_grid_tile_write (int qid)
        { if (m_ad)
                m_ad->enable_grid_tile_write(qid);
        }

        bool publish_grid_tile(grid_tile_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_grid_tile_write (qid))
                return m_ad->publish_grid_tile (d_, qid);
           else
                return false;
        }
        // *********** ground_wheel_angle *************
        void enable_ground_wheel_angle_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_ground_wheel_angle_read (qid))
                {
                        func = new func_ground_wheel_angle_t (&m_ground_wheel_angle[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_ground_wheel_angle_read (qid);
                }
        }

        bool get_ground_wheel_angle (ground_wheel_angle_data_t &d_, int qid)
        {
                if (m_ad && m_ad->ground_wheel_angle_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_ground_wheel_angle[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_ground_wheel_angle_write (int qid)
        { if (m_ad)
                m_ad->enable_ground_wheel_angle_write(qid);
        }

        bool publish_ground_wheel_angle(ground_wheel_angle_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_ground_wheel_angle_write (qid))
                return m_ad->publish_ground_wheel_angle (d_, qid);
           else
                return false;
        }
        // *********** guide *************
        void enable_guide_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_guide_read (qid))
                {
                        func = new func_guide_t (&m_guide[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_guide_read (qid);
                }
        }

        bool get_guide (guide_data_t &d_, int qid)
        {
                if (m_ad && m_ad->guide_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_guide[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_guide_write (int qid)
        { if (m_ad)
                m_ad->enable_guide_write(qid);
        }

        bool publish_guide(guide_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_guide_write (qid))
                return m_ad->publish_guide (d_, qid);
           else
                return false;
        }
        // *********** heading *************
        void enable_heading_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_heading_read (qid))
                {
                        func = new func_heading_t (&m_heading[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_heading_read (qid);
                }
        }

        bool get_heading (heading_data_t &d_, int qid)
        {
                if (m_ad && m_ad->heading_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_heading[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_heading_write (int qid)
        { if (m_ad)
                m_ad->enable_heading_write(qid);
        }

        bool publish_heading(heading_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_heading_write (qid))
                return m_ad->publish_heading (d_, qid);
           else
                return false;
        }
        // *********** ibeo_object *************
        void enable_ibeo_object_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_ibeo_object_read (qid))
                {
                        func = new func_ibeo_object_t (&m_ibeo_object[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_ibeo_object_read (qid);
                }
        }

        bool get_ibeo_object (ibeo_object_data_t &d_, int qid)
        {
                if (m_ad && m_ad->ibeo_object_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_ibeo_object[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_ibeo_object_write (int qid)
        { if (m_ad)
                m_ad->enable_ibeo_object_write(qid);
        }

        bool publish_ibeo_object(ibeo_object_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_ibeo_object_write (qid))
                return m_ad->publish_ibeo_object (d_, qid);
           else
                return false;
        }
        // *********** ibeo_scan *************
        void enable_ibeo_scan_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_ibeo_scan_read (qid))
                {
                        func = new func_ibeo_scan_t (&m_ibeo_scan[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_ibeo_scan_read (qid);
                }
        }

        bool get_ibeo_scan (ibeo_scan_data_t &d_, int qid)
        {
                if (m_ad && m_ad->ibeo_scan_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_ibeo_scan[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_ibeo_scan_write (int qid)
        { if (m_ad)
                m_ad->enable_ibeo_scan_write(qid);
        }

        bool publish_ibeo_scan(ibeo_scan_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_ibeo_scan_write (qid))
                return m_ad->publish_ibeo_scan (d_, qid);
           else
                return false;
        }
        // *********** image *************
        void enable_image_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_image_read (qid))
                {
                        func = new func_image_t (&m_image[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_image_read (qid);
                }
        }

        bool get_image (image_data_t &d_, int qid)
        {
                if (m_ad && m_ad->image_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_image[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_image_write (int qid)
        { if (m_ad)
                m_ad->enable_image_write(qid);
        }

        bool publish_image(image_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_image_write (qid))
                return m_ad->publish_image (d_, qid);
           else
                return false;
        }
        // *********** iteris_lane *************
        void enable_iteris_lane_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_iteris_lane_read (qid))
                {
                        func = new func_iteris_lane_t (&m_iteris_lane[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_iteris_lane_read (qid);
                }
        }

        bool get_iteris_lane (iteris_lane_data_t &d_, int qid)
        {
                if (m_ad && m_ad->iteris_lane_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_iteris_lane[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_iteris_lane_write (int qid)
        { if (m_ad)
                m_ad->enable_iteris_lane_write(qid);
        }

        bool publish_iteris_lane(iteris_lane_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_iteris_lane_write (qid))
                return m_ad->publish_iteris_lane (d_, qid);
           else
                return false;
        }
        // *********** iteris_lane_gp *************
        void enable_iteris_lane_gp_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_iteris_lane_gp_read (qid))
                {
                        func = new func_iteris_lane_gp_t (&m_iteris_lane_gp[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_iteris_lane_gp_read (qid);
                }
        }

        bool get_iteris_lane_gp (iteris_lane_gp_data_t &d_, int qid)
        {
                if (m_ad && m_ad->iteris_lane_gp_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_iteris_lane_gp[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_iteris_lane_gp_write (int qid)
        { if (m_ad)
                m_ad->enable_iteris_lane_gp_write(qid);
        }

        bool publish_iteris_lane_gp(iteris_lane_gp_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_iteris_lane_gp_write (qid))
                return m_ad->publish_iteris_lane_gp (d_, qid);
           else
                return false;
        }
        // *********** lane *************
        void enable_lane_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_lane_read (qid))
                {
                        func = new func_lane_t (&m_lane[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_lane_read (qid);
                }
        }

        bool get_lane (lane_data_t &d_, int qid)
        {
                if (m_ad && m_ad->lane_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_lane[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_lane_write (int qid)
        { if (m_ad)
                m_ad->enable_lane_write(qid);
        }

        bool publish_lane(lane_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_lane_write (qid))
                return m_ad->publish_lane (d_, qid);
           else
                return false;
        }
        // *********** local_dtm *************
        void enable_local_dtm_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_local_dtm_read (qid))
                {
                        func = new func_local_dtm_t (&m_local_dtm[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_local_dtm_read (qid);
                }
        }

        bool get_local_dtm (local_dtm_data_t &d_, int qid)
        {
                if (m_ad && m_ad->local_dtm_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_local_dtm[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_local_dtm_write (int qid)
        { if (m_ad)
                m_ad->enable_local_dtm_write(qid);
        }

        bool publish_local_dtm(local_dtm_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_local_dtm_write (qid))
                return m_ad->publish_local_dtm (d_, qid);
           else
                return false;
        }
        // *********** mesh_cell *************
        void enable_mesh_cell_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_mesh_cell_read (qid))
                {
                        func = new func_mesh_cell_t (&m_mesh_cell[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_mesh_cell_read (qid);
                }
        }

        bool get_mesh_cell (mesh_cell_data_t &d_, int qid)
        {
                if (m_ad && m_ad->mesh_cell_okay (qid))
                {
                        assert (qid >= 0 && qid < 4);
                        d_ = m_mesh_cell[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_mesh_cell_write (int qid)
        { if (m_ad)
                m_ad->enable_mesh_cell_write(qid);
        }

        bool publish_mesh_cell(mesh_cell_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_mesh_cell_write (qid))
                return m_ad->publish_mesh_cell (d_, qid);
           else
                return false;
        }
        // *********** mission_status *************
        void enable_mission_status_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_mission_status_read (qid))
                {
                        func = new func_mission_status_t (&m_mission_status[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_mission_status_read (qid);
                }
        }

        bool get_mission_status (mission_status_data_t &d_, int qid)
        {
                if (m_ad && m_ad->mission_status_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_mission_status[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_mission_status_write (int qid)
        { if (m_ad)
                m_ad->enable_mission_status_write(qid);
        }

        bool publish_mission_status(mission_status_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_mission_status_write (qid))
                return m_ad->publish_mission_status (d_, qid);
           else
                return false;
        }
        // *********** motion *************
        void enable_motion_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_motion_read (qid))
                {
                        func = new func_motion_t (&m_motion[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_motion_read (qid);
                }
        }

        bool get_motion (motion_data_t &d_, int qid)
        {
                if (m_ad && m_ad->motion_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_motion[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_motion_write (int qid)
        { if (m_ad)
                m_ad->enable_motion_write(qid);
        }

        bool publish_motion(motion_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_motion_write (qid))
                return m_ad->publish_motion (d_, qid);
           else
                return false;
        }
        // *********** nav *************
        void enable_nav_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_nav_read (qid))
                {
                        func = new func_nav_t (&m_nav[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_nav_read (qid);
                }
        }

        bool get_nav (nav_data_t &d_, int qid)
        {
                if (m_ad && m_ad->nav_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_nav[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_nav_write (int qid)
        { if (m_ad)
                m_ad->enable_nav_write(qid);
        }

        bool publish_nav(nav_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_nav_write (qid))
                return m_ad->publish_nav (d_, qid);
           else
                return false;
        }
        // *********** path *************
        void enable_path_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_path_read (qid))
                {
                        func = new func_path_t (&m_path[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_path_read (qid);
                }
        }

        bool get_path (path_data_t &d_, int qid)
        {
                if (m_ad && m_ad->path_okay (qid))
                {
                        assert (qid >= 0 && qid < 3);
                        d_ = m_path[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_path_write (int qid)
        { if (m_ad)
                m_ad->enable_path_write(qid);
        }

        bool publish_path(path_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_path_write (qid))
                return m_ad->publish_path (d_, qid);
           else
                return false;
        }
        // *********** plan_status *************
        void enable_plan_status_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_plan_status_read (qid))
                {
                        func = new func_plan_status_t (&m_plan_status[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_plan_status_read (qid);
                }
        }

        bool get_plan_status (plan_status_data_t &d_, int qid)
        {
                if (m_ad && m_ad->plan_status_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_plan_status[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_plan_status_write (int qid)
        { if (m_ad)
                m_ad->enable_plan_status_write(qid);
        }

        bool publish_plan_status(plan_status_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_plan_status_write (qid))
                return m_ad->publish_plan_status (d_, qid);
           else
                return false;
        }
        // *********** playback_control *************
        void enable_playback_control_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_playback_control_read (qid))
                {
                        func = new func_playback_control_t (&m_playback_control[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_playback_control_read (qid);
                }
        }

        bool get_playback_control (playback_control_data_t &d_, int qid)
        {
                if (m_ad && m_ad->playback_control_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_playback_control[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_playback_control_write (int qid)
        { if (m_ad)
                m_ad->enable_playback_control_write(qid);
        }

        bool publish_playback_control(playback_control_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_playback_control_write (qid))
                return m_ad->publish_playback_control (d_, qid);
           else
                return false;
        }
        // *********** radar *************
        void enable_radar_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_radar_read (qid))
                {
                        func = new func_radar_t (&m_radar[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_radar_read (qid);
                }
        }

        bool get_radar (radar_data_t &d_, int qid)
        {
                if (m_ad && m_ad->radar_okay (qid))
                {
                        assert (qid >= 0 && qid < 4);
                        d_ = m_radar[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_radar_write (int qid)
        { if (m_ad)
                m_ad->enable_radar_write(qid);
        }

        bool publish_radar(radar_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_radar_write (qid))
                return m_ad->publish_radar (d_, qid);
           else
                return false;
        }
        // *********** radar_gp *************
        void enable_radar_gp_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_radar_gp_read (qid))
                {
                        func = new func_radar_gp_t (&m_radar_gp[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_radar_gp_read (qid);
                }
        }

        bool get_radar_gp (radar_gp_data_t &d_, int qid)
        {
                if (m_ad && m_ad->radar_gp_okay (qid))
                {
                        assert (qid >= 0 && qid < 4);
                        d_ = m_radar_gp[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_radar_gp_write (int qid)
        { if (m_ad)
                m_ad->enable_radar_gp_write(qid);
        }

        bool publish_radar_gp(radar_gp_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_radar_gp_write (qid))
                return m_ad->publish_radar_gp (d_, qid);
           else
                return false;
        }
        // *********** range *************
        void enable_range_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_range_read (qid))
                {
                        func = new func_range_t (&m_range[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_range_read (qid);
                }
        }

        bool get_range (range_data_t &d_, int qid)
        {
                if (m_ad && m_ad->range_okay (qid))
                {
                        assert (qid >= 0 && qid < 3);
                        d_ = m_range[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_range_write (int qid)
        { if (m_ad)
                m_ad->enable_range_write(qid);
        }

        bool publish_range(range_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_range_write (qid))
                return m_ad->publish_range (d_, qid);
           else
                return false;
        }
        // *********** scan_analysis *************
        void enable_scan_analysis_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_scan_analysis_read (qid))
                {
                        func = new func_scan_analysis_t (&m_scan_analysis[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_scan_analysis_read (qid);
                }
        }

        bool get_scan_analysis (scan_analysis_data_t &d_, int qid)
        {
                if (m_ad && m_ad->scan_analysis_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_scan_analysis[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_scan_analysis_write (int qid)
        { if (m_ad)
                m_ad->enable_scan_analysis_write(qid);
        }

        bool publish_scan_analysis(scan_analysis_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_scan_analysis_write (qid))
                return m_ad->publish_scan_analysis (d_, qid);
           else
                return false;
        }
        // *********** scan *************
        void enable_scan_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_scan_read (qid))
                {
                        func = new func_scan_t (&m_scan[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_scan_read (qid);
                }
        }

        bool get_scan (scan_data_t &d_, int qid)
        {
                if (m_ad && m_ad->scan_okay (qid))
                {
                        assert (qid >= 0 && qid < 8);
                        d_ = m_scan[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_scan_write (int qid)
        { if (m_ad)
                m_ad->enable_scan_write(qid);
        }

        bool publish_scan(scan_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_scan_write (qid))
                return m_ad->publish_scan (d_, qid);
           else
                return false;
        }
        // *********** scan_dxyz *************
        void enable_scan_dxyz_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_scan_dxyz_read (qid))
                {
                        func = new func_scan_dxyz_t (&m_scan_dxyz[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_scan_dxyz_read (qid);
                }
        }

        bool get_scan_dxyz (scan_dxyz_data_t &d_, int qid)
        {
                if (m_ad && m_ad->scan_dxyz_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_scan_dxyz[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_scan_dxyz_write (int qid)
        { if (m_ad)
                m_ad->enable_scan_dxyz_write(qid);
        }

        bool publish_scan_dxyz(scan_dxyz_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_scan_dxyz_write (qid))
                return m_ad->publish_scan_dxyz (d_, qid);
           else
                return false;
        }
        // *********** scan_gp *************
        void enable_scan_gp_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_scan_gp_read (qid))
                {
                        func = new func_scan_gp_t (&m_scan_gp[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_scan_gp_read (qid);
                }
        }

        bool get_scan_gp (scan_gp_data_t &d_, int qid)
        {
                if (m_ad && m_ad->scan_gp_okay (qid))
                {
                        assert (qid >= 0 && qid < 8);
                        d_ = m_scan_gp[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_scan_gp_write (int qid)
        { if (m_ad)
                m_ad->enable_scan_gp_write(qid);
        }

        bool publish_scan_gp(scan_gp_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_scan_gp_write (qid))
                return m_ad->publish_scan_gp (d_, qid);
           else
                return false;
        }
        // *********** scan_lane_gp *************
        void enable_scan_lane_gp_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_scan_lane_gp_read (qid))
                {
                        func = new func_scan_lane_gp_t (&m_scan_lane_gp[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_scan_lane_gp_read (qid);
                }
        }

        bool get_scan_lane_gp (scan_lane_gp_data_t &d_, int qid)
        {
                if (m_ad && m_ad->scan_lane_gp_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_scan_lane_gp[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_scan_lane_gp_write (int qid)
        { if (m_ad)
                m_ad->enable_scan_lane_gp_write(qid);
        }

        bool publish_scan_lane_gp(scan_lane_gp_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_scan_lane_gp_write (qid))
                return m_ad->publish_scan_lane_gp (d_, qid);
           else
                return false;
        }
        // *********** sensor_position *************
        void enable_sensor_position_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_sensor_position_read (qid))
                {
                        func = new func_sensor_position_t (&m_sensor_position[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_sensor_position_read (qid);
                }
        }

        bool get_sensor_position (sensor_position_data_t &d_, int qid)
        {
                if (m_ad && m_ad->sensor_position_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_sensor_position[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_sensor_position_write (int qid)
        { if (m_ad)
                m_ad->enable_sensor_position_write(qid);
        }

        bool publish_sensor_position(sensor_position_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_sensor_position_write (qid))
                return m_ad->publish_sensor_position (d_, qid);
           else
                return false;
        }
        // *********** signal_line *************
        void enable_signal_line_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_signal_line_read (qid))
                {
                        func = new func_signal_line_t (&m_signal_line[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_signal_line_read (qid);
                }
        }

        bool get_signal_line (signal_line_data_t &d_, int qid)
        {
                if (m_ad && m_ad->signal_line_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_signal_line[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_signal_line_write (int qid)
        { if (m_ad)
                m_ad->enable_signal_line_write(qid);
        }

        bool publish_signal_line(signal_line_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_signal_line_write (qid))
                return m_ad->publish_signal_line (d_, qid);
           else
                return false;
        }
        // *********** sim_time *************
        void enable_sim_time_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_sim_time_read (qid))
                {
                        func = new func_sim_time_t (&m_sim_time[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_sim_time_read (qid);
                }
        }

        bool get_sim_time (sim_time_data_t &d_, int qid)
        {
                if (m_ad && m_ad->sim_time_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_sim_time[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_sim_time_write (int qid)
        { if (m_ad)
                m_ad->enable_sim_time_write(qid);
        }

        bool publish_sim_time(sim_time_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_sim_time_write (qid))
                return m_ad->publish_sim_time (d_, qid);
           else
                return false;
        }
        // *********** sog *************
        void enable_sog_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_sog_read (qid))
                {
                        func = new func_sog_t (&m_sog[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_sog_read (qid);
                }
        }

        bool get_sog (sog_data_t &d_, int qid)
        {
                if (m_ad && m_ad->sog_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_sog[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_sog_write (int qid)
        { if (m_ad)
                m_ad->enable_sog_write(qid);
        }

        bool publish_sog(sog_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_sog_write (qid))
                return m_ad->publish_sog (d_, qid);
           else
                return false;
        }
        // *********** steering *************
        void enable_steering_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_steering_read (qid))
                {
                        func = new func_steering_t (&m_steering[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_steering_read (qid);
                }
        }

        bool get_steering (steering_data_t &d_, int qid)
        {
                if (m_ad && m_ad->steering_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_steering[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_steering_write (int qid)
        { if (m_ad)
                m_ad->enable_steering_write(qid);
        }

        bool publish_steering(steering_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_steering_write (qid))
                return m_ad->publish_steering (d_, qid);
           else
                return false;
        }
        // *********** survey_gp *************
        void enable_survey_gp_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_survey_gp_read (qid))
                {
                        func = new func_survey_gp_t (&m_survey_gp[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_survey_gp_read (qid);
                }
        }

        bool get_survey_gp (survey_gp_data_t &d_, int qid)
        {
                if (m_ad && m_ad->survey_gp_okay (qid))
                {
                        assert (qid >= 0 && qid < 2);
                        d_ = m_survey_gp[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_survey_gp_write (int qid)
        { if (m_ad)
                m_ad->enable_survey_gp_write(qid);
        }

        bool publish_survey_gp(survey_gp_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_survey_gp_write (qid))
                return m_ad->publish_survey_gp (d_, qid);
           else
                return false;
        }
        // *********** triangle_data *************
        void enable_triangle_data_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_triangle_data_read (qid))
                {
                        func = new func_triangle_data_t (&m_triangle_data[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_triangle_data_read (qid);
                }
        }

        bool get_triangle_data (triangle_data_t &d_, int qid)
        {
                if (m_ad && m_ad->triangle_data_okay (qid))
                {
                        assert (qid >= 0 && qid < 4);
                        d_ = m_triangle_data[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_triangle_data_write (int qid)
        { if (m_ad)
                m_ad->enable_triangle_data_write(qid);
        }

        bool publish_triangle_data(triangle_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_triangle_data_write (qid))
                return m_ad->publish_triangle_data (d_, qid);
           else
                return false;
        }
        // *********** vehicle *************
        void enable_vehicle_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_vehicle_read (qid))
                {
                        func = new func_vehicle_t (&m_vehicle[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_vehicle_read (qid);
                }
        }

        bool get_vehicle (vehicle_data_t &d_, int qid)
        {
                if (m_ad && m_ad->vehicle_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_vehicle[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_vehicle_write (int qid)
        { if (m_ad)
                m_ad->enable_vehicle_write(qid);
        }

        bool publish_vehicle(vehicle_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_vehicle_write (qid))
                return m_ad->publish_vehicle (d_, qid);
           else
                return false;
        }
        // *********** vpath *************
        void enable_vpath_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_vpath_read (qid))
                {
                        func = new func_vpath_t (&m_vpath[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_vpath_read (qid);
                }
        }

        bool get_vpath (vpath_data_t &d_, int qid)
        {
                if (m_ad && m_ad->vpath_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_vpath[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_vpath_write (int qid)
        { if (m_ad)
                m_ad->enable_vpath_write(qid);
        }

        bool publish_vpath(vpath_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_vpath_write (qid))
                return m_ad->publish_vpath (d_, qid);
           else
                return false;
        }
        // *********** waypt *************
        void enable_waypt_read (int qid)
        {
                if (m_ad && ! m_ad->is_enable_waypt_read (qid))
                {
                        func = new func_waypt_t (&m_waypt[qid], qid, m_ad);
                        m_func_vector.push_back (func);
                        func = NULL;
                        m_ad->enable_waypt_read (qid);
                }
        }

        bool get_waypt (waypt_data_t &d_, int qid)
        {
                if (m_ad && m_ad->waypt_okay (qid))
                {
                        assert (qid >= 0 && qid < 1);
                        d_ = m_waypt[qid];
                        return true;
                }
                else
                        return false;
        }

        void enable_waypt_write (int qid)
        { if (m_ad)
                m_ad->enable_waypt_write(qid);
        }

        bool publish_waypt(waypt_data_t &d_, int qid)
        {
           if (m_ad && m_ad->is_enable_waypt_write (qid))
                return m_ad->publish_waypt (d_, qid);
           else
                return false;
        }

        void update_state (bool debug= false);

        bool input_data_available ()
        {
                while (true)
                {
                        if (
                        (!m_ad->is_enable_acceleration_read (0) ||
                                  m_ad->acceleration_okay (0)) &&
                        (!m_ad->is_enable_angular_rate_read (0) ||
                                  m_ad->angular_rate_okay (0)) &&
                        (!m_ad->is_enable_cbln_read (0) ||
                                  m_ad->cbln_okay (0)) &&
                        (!m_ad->is_enable_cbln_dtm_read (0) ||
                                  m_ad->cbln_dtm_okay (0)) &&
                        (!m_ad->is_enable_cbmesg_read (0) ||
                                  m_ad->cbmesg_okay (0)) &&
                        (!m_ad->is_enable_cog_read (0) ||
                                  m_ad->cog_okay (0)) &&
                        (!m_ad->is_enable_control_read (0) ||
                                  m_ad->control_okay (0)) &&
                        (!m_ad->is_enable_control_line_read (0) ||
                                  m_ad->control_line_okay (0)) &&
                        (!m_ad->is_enable_drive_read (0) ||
                                  m_ad->drive_okay (0)) &&
                        (!m_ad->is_enable_grid_path_read (0) ||
                                  m_ad->grid_path_okay (0)) &&
                        (!m_ad->is_enable_grid_tile_read (0) ||
                                  m_ad->grid_tile_okay (0)) &&
                        (!m_ad->is_enable_ground_wheel_angle_read (0) ||
                                  m_ad->ground_wheel_angle_okay (0)) &&
                        (!m_ad->is_enable_guide_read (0) ||
                                  m_ad->guide_okay (0)) &&
                        (!m_ad->is_enable_heading_read (0) ||
                                  m_ad->heading_okay (0)) &&
                        (!m_ad->is_enable_ibeo_object_read (0) ||
                                  m_ad->ibeo_object_okay (0)) &&
                        (!m_ad->is_enable_ibeo_scan_read (0) ||
                                  m_ad->ibeo_scan_okay (0)) &&
                        (!m_ad->is_enable_image_read (0) ||
                                  m_ad->image_okay (0)) &&
                        (!m_ad->is_enable_iteris_lane_read (0) ||
                                  m_ad->iteris_lane_okay (0)) &&
                        (!m_ad->is_enable_iteris_lane_gp_read (0) ||
                                  m_ad->iteris_lane_gp_okay (0)) &&
                        (!m_ad->is_enable_lane_read (0) ||
                                  m_ad->lane_okay (0)) &&
                        (!m_ad->is_enable_local_dtm_read (0) ||
                                  m_ad->local_dtm_okay (0)) &&
                        (!m_ad->is_enable_mesh_cell_read (0) ||
                                  m_ad->mesh_cell_okay (0)) &&
                        (!m_ad->is_enable_mission_status_read (0) ||
                                  m_ad->mission_status_okay (0)) &&
                        (!m_ad->is_enable_motion_read (0) ||
                                  m_ad->motion_okay (0)) &&
                        (!m_ad->is_enable_nav_read (0) ||
                                  m_ad->nav_okay (0)) &&
                        (!m_ad->is_enable_path_read (0) ||
                                  m_ad->path_okay (0)) &&
                        (!m_ad->is_enable_plan_status_read (0) ||
                                  m_ad->plan_status_okay (0)) &&
                        (!m_ad->is_enable_playback_control_read (0) ||
                                  m_ad->playback_control_okay (0)) &&
                        (!m_ad->is_enable_radar_read (0) ||
                                  m_ad->radar_okay (0)) &&
                        (!m_ad->is_enable_radar_gp_read (0) ||
                                  m_ad->radar_gp_okay (0)) &&
                        (!m_ad->is_enable_range_read (0) ||
                                  m_ad->range_okay (0)) &&
                        (!m_ad->is_enable_scan_analysis_read (0) ||
                                  m_ad->scan_analysis_okay (0)) &&
                        (!m_ad->is_enable_scan_read (0) ||
                                  m_ad->scan_okay (0)) &&
                        (!m_ad->is_enable_scan_dxyz_read (0) ||
                                  m_ad->scan_dxyz_okay (0)) &&
                        (!m_ad->is_enable_scan_gp_read (0) ||
                                  m_ad->scan_gp_okay (0)) &&
                        (!m_ad->is_enable_scan_lane_gp_read (0) ||
                                  m_ad->scan_lane_gp_okay (0)) &&
                        (!m_ad->is_enable_sensor_position_read (0) ||
                                  m_ad->sensor_position_okay (0)) &&
                        (!m_ad->is_enable_signal_line_read (0) ||
                                  m_ad->signal_line_okay (0)) &&
                        (!m_ad->is_enable_sim_time_read (0) ||
                                  m_ad->sim_time_okay (0)) &&
                        (!m_ad->is_enable_sog_read (0) ||
                                  m_ad->sog_okay (0)) &&
                        (!m_ad->is_enable_steering_read (0) ||
                                  m_ad->steering_okay (0)) &&
                        (!m_ad->is_enable_survey_gp_read (0) ||
                                  m_ad->survey_gp_okay (0)) &&
                        (!m_ad->is_enable_triangle_data_read (0) ||
                                  m_ad->triangle_data_okay (0)) &&
                        (!m_ad->is_enable_vehicle_read (0) ||
                                  m_ad->vehicle_okay (0)) &&
                        (!m_ad->is_enable_vpath_read (0) ||
                                  m_ad->vpath_okay (0)) &&
                        (!m_ad->is_enable_waypt_read (0) ||
                                  m_ad->waypt_okay (0)) &&
                                true)
                                break;
                        cb_time_t::wait_until (cb_time_t::curr_time() + 0.05);
                }
                return true;
        }

        bool input_acceleration_available (unsigned qid_)
        {
                return (m_ad->acceleration_okay (qid_));
        }
        bool input_angular_rate_available (unsigned qid_)
        {
                return (m_ad->angular_rate_okay (qid_));
        }
        bool input_cbln_available (unsigned qid_)
        {
                return (m_ad->cbln_okay (qid_));
        }
        bool input_cbln_dtm_available (unsigned qid_)
        {
                return (m_ad->cbln_dtm_okay (qid_));
        }
        bool input_cbmesg_available (unsigned qid_)
        {
                return (m_ad->cbmesg_okay (qid_));
        }
        bool input_cog_available (unsigned qid_)
        {
                return (m_ad->cog_okay (qid_));
        }
        bool input_control_available (unsigned qid_)
        {
                return (m_ad->control_okay (qid_));
        }
        bool input_control_line_available (unsigned qid_)
        {
                return (m_ad->control_line_okay (qid_));
        }
        bool input_drive_available (unsigned qid_)
        {
                return (m_ad->drive_okay (qid_));
        }
        bool input_grid_path_available (unsigned qid_)
        {
                return (m_ad->grid_path_okay (qid_));
        }
        bool input_grid_tile_available (unsigned qid_)
        {
                return (m_ad->grid_tile_okay (qid_));
        }
        bool input_ground_wheel_angle_available (unsigned qid_)
        {
                return (m_ad->ground_wheel_angle_okay (qid_));
        }
        bool input_guide_available (unsigned qid_)
        {
                return (m_ad->guide_okay (qid_));
        }
        bool input_heading_available (unsigned qid_)
        {
                return (m_ad->heading_okay (qid_));
        }
        bool input_ibeo_object_available (unsigned qid_)
        {
                return (m_ad->ibeo_object_okay (qid_));
        }
        bool input_ibeo_scan_available (unsigned qid_)
        {
                return (m_ad->ibeo_scan_okay (qid_));
        }
        bool input_image_available (unsigned qid_)
        {
                return (m_ad->image_okay (qid_));
        }
        bool input_iteris_lane_available (unsigned qid_)
        {
                return (m_ad->iteris_lane_okay (qid_));
        }
        bool input_iteris_lane_gp_available (unsigned qid_)
        {
                return (m_ad->iteris_lane_gp_okay (qid_));
        }
        bool input_lane_available (unsigned qid_)
        {
                return (m_ad->lane_okay (qid_));
        }
        bool input_local_dtm_available (unsigned qid_)
        {
                return (m_ad->local_dtm_okay (qid_));
        }
        bool input_mesh_cell_available (unsigned qid_)
        {
                return (m_ad->mesh_cell_okay (qid_));
        }
        bool input_mission_status_available (unsigned qid_)
        {
                return (m_ad->mission_status_okay (qid_));
        }
        bool input_motion_available (unsigned qid_)
        {
                return (m_ad->motion_okay (qid_));
        }
        bool input_nav_available (unsigned qid_)
        {
                return (m_ad->nav_okay (qid_));
        }
        bool input_path_available (unsigned qid_)
        {
                return (m_ad->path_okay (qid_));
        }
        bool input_plan_status_available (unsigned qid_)
        {
                return (m_ad->plan_status_okay (qid_));
        }
        bool input_playback_control_available (unsigned qid_)
        {
                return (m_ad->playback_control_okay (qid_));
        }
        bool input_radar_available (unsigned qid_)
        {
                return (m_ad->radar_okay (qid_));
        }
        bool input_radar_gp_available (unsigned qid_)
        {
                return (m_ad->radar_gp_okay (qid_));
        }
        bool input_range_available (unsigned qid_)
        {
                return (m_ad->range_okay (qid_));
        }
        bool input_scan_analysis_available (unsigned qid_)
        {
                return (m_ad->scan_analysis_okay (qid_));
        }
        bool input_scan_available (unsigned qid_)
        {
                return (m_ad->scan_okay (qid_));
        }
        bool input_scan_dxyz_available (unsigned qid_)
        {
                return (m_ad->scan_dxyz_okay (qid_));
        }
        bool input_scan_gp_available (unsigned qid_)
        {
                return (m_ad->scan_gp_okay (qid_));
        }
        bool input_scan_lane_gp_available (unsigned qid_)
        {
                return (m_ad->scan_lane_gp_okay (qid_));
        }
        bool input_sensor_position_available (unsigned qid_)
        {
                return (m_ad->sensor_position_okay (qid_));
        }
        bool input_signal_line_available (unsigned qid_)
        {
                return (m_ad->signal_line_okay (qid_));
        }
        bool input_sim_time_available (unsigned qid_)
        {
                return (m_ad->sim_time_okay (qid_));
        }
        bool input_sog_available (unsigned qid_)
        {
                return (m_ad->sog_okay (qid_));
        }
        bool input_steering_available (unsigned qid_)
        {
                return (m_ad->steering_okay (qid_));
        }
        bool input_survey_gp_available (unsigned qid_)
        {
                return (m_ad->survey_gp_okay (qid_));
        }
        bool input_triangle_data_available (unsigned qid_)
        {
                return (m_ad->triangle_data_okay (qid_));
        }
        bool input_vehicle_available (unsigned qid_)
        {
                return (m_ad->vehicle_okay (qid_));
        }
        bool input_vpath_available (unsigned qid_)
        {
                return (m_ad->vpath_okay (qid_));
        }
        bool input_waypt_available (unsigned qid_)
        {
                return (m_ad->waypt_okay (qid_));
        }

private:

        double m_nav_atrack; //atrack dist of front of the vehicle from INS pos
        double m_nav_xtrack; //xtrack dist of front of the vehicle from INS pos

public:
        bool get_bumper_nav (nav_data_t &nav_);

        //****** Spatial to rndf map ******************
        spatial_to_lane_map_t *m_spatial_map;
        void enable_spatial_map(const rndf_t *rndf_, double cell_size_)
        {
                assert (m_rndf_lane_container);

                if (rndf_ != NULL && m_rndf == NULL)
                        m_rndf = rndf_;

                if (m_spatial_map)
                        return;
                        
                m_spatial_map = new spatial_to_lane_map_t (m_rndf,
                                                           cell_size_);

                std::vector<rndf_lane_data_t> rndf_lanes;

                get_all_lane_segments(rndf_lanes);
                assert (m_spatial_map);
                m_spatial_map->update_spatial_map(rndf_lanes);
        }
        bool neighboring_lanes (std::vector<rndf_t::lane_seg_t> &lanes_seg,
                                double x_, double y_, double rad_)
        {
                assert (m_spatial_map);
                return m_spatial_map->neighboring_lanes (lanes_seg, x_, y_,
                                                        rad_);
        }

        bool near_any_lane(double x_, double y_, double rad_)
        { return m_spatial_map->near_any_lane(x_, y_, rad_); }


        //****** RNDF related function ******************
        bool m_map_rndf_lane;
        rndf_lane_container_t *m_rndf_lane_container;

        // ******** rndf lane boundary ***********
        void enable_rndf_lane_map (rndf_t const *rndf_ = NULL,
                                   double interp_dis_ = 1, unsigned qid_ = 0);

        //rndf lane is excluded from template list, adding functions manually*/
        bool publish_rndf_lane (rndf_lane_data_t &rl_, int qid);

        void enable_rndf_lane_write (int qid);

        void wait_for_stable_world ();

        bool update_rndf_lanes (std::vector<rndf_lane_data_t> &rndf_list_);
        void update_rndf_lane (rndf_lane_data_t &rndf_lane);

        void get_all_lane_segments(std::vector<rndf_lane_data_t> &rndf_lanes);

        bool get_lane_boundary (rndf_lane_data_t &lb_,unsigned seg_id_,
                                unsigned l_id_);

        bool stop_region_blocked (rndf_t::waypoint_t const &wp_,
                                double &blk_tstamp_);

        bool get_lane_seg (rndf_lane_data_t &lb_,
                           unsigned start_sid_, unsigned start_lid_,
                           unsigned start_wid_, unsigned end_sid_,
                           unsigned end_lid_, unsigned end_wid_);

        // ****** General & NON template ***************
private:
        const rndf_t *m_rndf;


        //***** Radar data related ***************
        static const unsigned NUM_RADAR_QUEUES = 4;
        bool m_buffer_radar [NUM_RADAR_QUEUES];
        radar_data_collector_t *m_radar_collector[NUM_RADAR_QUEUES];
public:
        void enable_radar_data_buffer(unsigned qid_);
        
        bool update_radar_data (unsigned qid_, bool debug= FALSE);

        bool get_radar_data (unsigned qid_,
                        std::vector<radar_object_t> &radar_list_,
                        double threshold_tstamp_) const;


        //***** Blocked lane related ***************
private:
        bool m_buffer_blocked_lanes;
        blocked_lane_container_t *m_blk_lane_container;
public:
        void enable_blocked_lane_buffer();
        void enable_blocked_lane_write (int qid);

        bool publish_blocked_lane (blocked_lane_data_t &blk_lane, int qid);

        bool update_blocked_lanes (unsigned qid_);
        bool update_blocked_lanes (
                const std::vector<blocked_lane_data_t> &blk_lanes_);
        void update_blocked_lane (const blocked_lane_data_t &blk_lane_);

        /*\brief Returns all blocked_lane_data for all lanes */
        bool get_all_blockages (
                std::vector<blocked_lane_data_t> &blk_lanes_,
                double threshold_tstamp_) const;
        /*\brief Returns all blocked_lane_data for specified lane */
        bool get_lane_blockages (
                std::vector<blocked_lane_data_t> &blk_lanes_, unsigned sid,
                unsigned lid, double threshold_tstamp_) const;



        //******* Intersection tracking related ***********
private:
        bool m_track_intersection;
        intersection_tracker_t *m_intersection_tracker;
public:
        bool update_intersection (double threshold_tstamp_);
        void enable_intersection_tracking(const rndf_t *rndf_ = NULL);
        const intersection_tracker_t::intersection_status_t* get_intersection_status(rndf_t::waypoint_id_t wp_id_);



        //***** Obstacle related *****
private:
        static const unsigned NUM_OBST_QUEUES = 6;
        bool m_buffer_obstacle [NUM_OBST_QUEUES];
        tstamp_buffering_t<obstacle_data_t> m_obstacle_container [NUM_OBST_QUEUES];
public:
        void enable_obstacle_buffer (unsigned qid);
        void enable_obstacle_write (unsigned qid);

        void publish_obstacle (obstacle_data_t &obstacle_, unsigned qid);
        void publish_obstacle (std::vector<obstacle_data_t> obst_list,
                                unsigned qid);

        bool update_obstacle (unsigned qid);
        bool update_obstacle (std::vector <obstacle_data_t> &obst_list,
                                unsigned qid);

        bool get_all_obstacles (std::vector<obstacle_data_t> &obst_list_,
                                unsigned qid);

        //***** Obstacle point related *****
private:
        static const unsigned NUM_OBST_POINT_QUEUES = 6;
        bool m_buffer_obstacle_points [NUM_OBST_POINT_QUEUES];
        tstamp_buffering_t<obstacle_points_data_t> m_obstacle_points_container [NUM_OBST_POINT_QUEUES];
public:
        void enable_obstacle_points_buffer (unsigned qid);
        void enable_obstacle_points_write (unsigned qid);

        void publish_obstacle_points (obstacle_points_data_t &obstacle_, unsigned qid);
        void publish_obstacle_points (std::vector<obstacle_points_data_t> obst_list,
                                        unsigned qid);

        bool update_obstacle_points (unsigned qid);
        bool update_obstacle_points (std::vector <obstacle_points_data_t> &obst_list,
                                unsigned qid);

        bool get_all_obstacle_points (std::vector<obstacle_points_data_t> &obst_list_,
                                unsigned qid);

        bool get_zone_obstacles(
                std::vector<obstacle_points_data_t> &obst_list_,
                unsigned zone_id, unsigned qid_,
                obstacle_points_data_t::obstacle_mobility_t for_mobility_);
        bool obstacle_in_zone (obstacle_points_data_t const &obst_,
                                               rndf_t::zone_t const &zone);

        //***** scan_gp point related *****
private:
        static const unsigned NUM_SCAN_GP_QUEUES = 8;
        bool m_buffer_scan_gp [NUM_SCAN_GP_QUEUES];
        tstamp_buffering_t<scan_gp_data_t> m_scan_gp_container [NUM_SCAN_GP_QUEUES];
public:
        void enable_scan_gp_buffer (unsigned qid);
        void enable_scan_gp_write (unsigned qid);

        void publish_scan_gp (scan_gp_data_t &obstacle_, unsigned qid);
        void publish_scan_gp (std::vector<scan_gp_data_t> obst_list,
                                        unsigned qid);

        bool update_scan_gp (unsigned qid);
        bool update_scan_gp (std::vector <scan_gp_data_t> &obst_list,
                                unsigned qid);

        bool get_all_scan_gp (std::vector<scan_gp_data_t> &obst_list_,
                                unsigned qid);
};

};

#endif

---