/** * @brief set configuration parameter * @param [out] dest : configuration parameter declaration * @param [int] src : configuration parameter */ inline int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::set_parameter(dest, &src->kfile); gnd::conf::set_parameter(dest, &src->output_ssmname); gnd::conf::set_parameter(dest, &src->output_ssmid); gnd::conf::set_parameter(dest, &src->motor_ssmname); gnd::conf::set_parameter(dest, &src->motor_ssmid); gnd::conf::set_parameter(dest, &src->odmerr_name); gnd::conf::set_parameter(dest, &src->odmerr_id); gnd::conf::set_parameter(dest, &src->ad_ssmname); gnd::conf::set_parameter(dest, &src->ad_ssmid); gnd::conf::set_parameter(dest, &src->ad_bits); gnd::conf::set_parameter(dest, &src->ratio_port); gnd::conf::set_parameter(dest, &src->voltage); gnd::conf::set_parameter(dest, &src->bias); gnd::conf::set_parameter(dest, &src->scale_factor); gnd::conf::get_parameter(dest, &src->spur_adjust); gnd::conf::set_parameter(dest, &src->gyrodometry); gnd::conf::set_parameter(dest, &src->error_simulation); gnd::conf::set_parameter(dest, &src->error_distributuion); gnd::conf::set_parameter(dest, &src->debug); return 0; }
/** * @brief get configuration parameter * @param [in] src : configuration parameter declaration * @param [out] dest : configuration parameter */ inline int proc_conf_get(gnd::conf::configuration *src, proc_configuration* dest) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::get_parameter(src, &dest->bmp_map); gnd::conf::get_parameter(src, &dest->raw_map); gnd::conf::get_parameter(src, &dest->sokuikiraw_name); gnd::conf::get_parameter(src, &dest->sokuikiraw_id); gnd::conf::get_parameter(src, &dest->sokuikifs_name); gnd::conf::get_parameter(src, &dest->sokuikifs_id); gnd::conf::get_parameter(src, &dest->particle_name); gnd::conf::get_parameter(src, &dest->particle_id); gnd::conf::get_parameter(src, &dest->odometry_name); gnd::conf::get_parameter(src, &dest->odometry_id); gnd::conf::get_parameter(src, &dest->eval_name); gnd::conf::get_parameter(src, &dest->eval_id); gnd::conf::get_parameter(src, &dest->cycle); gnd::conf::get_parameter(src, &dest->sleeping_time); gnd::conf::get_parameter(src, &dest->sleeping_dist); gnd::conf::get_parameter(src, &dest->cull); gnd::conf::get_parameter(src, &dest->blur); gnd::conf::get_parameter(src, &dest->scan_range); gnd::conf::get_parameter(src, &dest->matching_false); gnd::conf::get_parameter(src, &dest->false_positive); if( gnd::conf::get_parameter(src, &dest->sleeping_orient) >= 0 ){ // convert unit of angle(deg2rad) dest->sleeping_orient.value = gnd_deg2rad(dest->sleeping_orient.value); } return 0; }
/** * @brief set configuration parameter declaration * @param [out] dest : configuration parameter declaration * @param [in] src : configuration parameter */ inline int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::set_parameter(dest, &src->bmp_map); gnd::conf::set_parameter(dest, &src->raw_map); gnd::conf::set_parameter(dest, &src->sokuikiraw_name); gnd::conf::set_parameter(dest, &src->sokuikiraw_id); gnd::conf::set_parameter(dest, &src->odometry_name); gnd::conf::set_parameter(dest, &src->odometry_id); gnd::conf::set_parameter(dest, &src->particle_name); gnd::conf::set_parameter(dest, &src->particle_id); gnd::conf::set_parameter(dest, &src->eval_name); gnd::conf::set_parameter(dest, &src->eval_id); gnd::conf::set_parameter(dest, &src->cycle); gnd::conf::set_parameter(dest, &src->sleeping_time); gnd::conf::set_parameter(dest, &src->sleeping_dist); // convert unit of angle (rad2deg) src->sleeping_orient.value = gnd_rad2deg(src->sleeping_orient.value); gnd::conf::set_parameter(dest, &src->sleeping_orient); // reconvert unit of angle (rad2deg) src->sleeping_orient.value = gnd_deg2rad(src->sleeping_orient.value); gnd::conf::set_parameter(dest, &src->cull); gnd::conf::set_parameter(dest, &src->blur); gnd::conf::set_parameter(dest, &src->scan_range); gnd::conf::set_parameter(dest, &src->mfailure); return 0; }
/** * @brief file out configure file */ inline int proc_conf_write( const char* f, proc_configuration *src ){ gnd_assert(!f, -1, "invalid null pointer"); gnd_assert(!src, -1, "invalid null pointer"); { // ---> operation int ret; gnd::conf::file_stream fs; if( (ret = proc_conf_set(&fs, src)) < 0 ) return ret; return fs.write(f); } // <--- operation }
/** * @brief read configuration parameter file * @param [in] f : configuration file name * @param [out] dest : configuration parameter */ inline int proc_conf_read(const char* f, proc_configuration* dest) { gnd_assert(!f, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); { // ---> operation int ret; gnd::conf::file_stream fs; // configuration file read if( (ret = fs.read(f)) < 0 ) return ret; return proc_conf_get(&fs, dest); } // <--- operation }
/** * @brief get configuration parameter * @param [in] src : configuration * @param [out] dest : configuration parameter */ inline int proc_conf_get( gnd::conf::configuration* src, proc_configuration* dest ) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::get_parameter( src, &dest->init_opsm_map ); gnd::conf::get_parameter( src, &dest->cmap ); gnd::conf::get_parameter( src, &dest->culling ); gnd::conf::get_parameter( src, &dest->cycle ); gnd::conf::get_parameter( src, &dest->failure_dist ); if( !gnd::conf::get_parameter( src, &dest->failure_orient) ) dest->failure_orient.value = gnd_deg2ang(dest->failure_orient.value); gnd::conf::get_parameter( src, &dest->pause_time ); gnd::conf::get_parameter( src, &dest->pause_dist ); if( !gnd::conf::get_parameter( src, &dest->pause_orient ) ) dest->pause_orient.value = gnd_deg2ang(dest->pause_orient.value); gnd::conf::get_parameter( src, &dest->map_update ); gnd::conf::get_parameter( src, &dest->map_update_time ); gnd::conf::get_parameter( src, &dest->map_update_dist ); if( !gnd::conf::get_parameter( src, &dest->map_update_orient) ) dest->converge_orient.value = gnd_deg2ang(dest->map_update_orient.value); gnd::conf::get_parameter( src, &dest->optimizer ); gnd::conf::get_parameter( src, &dest->converge_dist ); if( !gnd::conf::get_parameter( src, &dest->converge_orient) ) dest->converge_orient.value = gnd_deg2ang(dest->converge_orient.value); gnd::conf::get_parameter( src, &dest->ini_map_cnt ); gnd::conf::get_parameter( src, &dest->ini_match_cnt ); gnd::conf::get_parameter( src, &dest->ndt ); gnd::conf::get_parameter( src, &dest->cui_show ); gnd::conf::get_parameter( src, &dest->opsm_map ); gnd::conf::get_parameter( src, &dest->bmp ); gnd::conf::get_parameter( src, &dest->pos_gridsizex ); gnd::conf::get_parameter( src, &dest->pos_gridsizey ); gnd::conf::get_parameter( src, &dest->ang_rsl ); gnd::conf::get_parameter( src, &dest->odm_logname ); gnd::conf::get_parameter( src, &dest->ls_logname ); gnd::conf::get_parameter( src, &dest->trajectory_log ); gnd::conf::get_parameter( src, &dest->trajectory4route ); gnd::conf::get_parameter( src, &dest->laserpoint_log ); gnd::conf::get_parameter( src, &dest->output_dir ); gnd::conf::get_parameter( src, &dest->debug_odo_err_map ); return 0; }
// configure file data analyze inline int proc_conf_get(gnd::conf::configuration *src, proc_configuration* dest) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::get_parameter(src, &dest->kfile); gnd::conf::get_parameter(src, &dest->k_rwheel); gnd::conf::get_parameter(src, &dest->k_rwheel_crot); gnd::conf::get_parameter(src, &dest->k_lwheel); gnd::conf::get_parameter(src, &dest->k_lwheel_crot); gnd::conf::get_parameter(src, &dest->k_tread); gnd::conf::get_parameter(src, &dest->k_gear); gnd::conf::get_parameter(src, &dest->k_encoder); gnd::conf::get_parameter(src, &dest->particles); if( gnd::conf::get_parameter(src, &dest->pos_err_ini ) >= 3) { dest->pos_err_ini.value[2] = gnd_deg2ang(dest->pos_err_ini.value[2]); } gnd::conf::get_parameter(src, &dest->sys_err_ini); gnd::conf::get_parameter(src, &dest->resmp_rate_remain); gnd::conf::get_parameter(src, &dest->resmp_rate_randerr); if( gnd::conf::get_parameter(src, &dest->randerr_conf ) >= 3) { dest->randerr_conf.value[2] = gnd_deg2ang(dest->randerr_conf.value[2]); } if( gnd::conf::get_parameter(src, &dest->randerr_offset_conf ) >= 3) { dest->randerr_offset_conf.value[2] = gnd_deg2ang(dest->randerr_offset_conf.value[2]); } gnd::conf::get_parameter(src, &dest->syserr_conf ); gnd::conf::get_parameter(src, &dest->resmp_rate_syserr2); gnd::conf::get_parameter(src, &dest->syserr2_conf ); gnd::conf::get_parameter(src, &dest->resmp_rate_resetsyserr); gnd::conf::get_parameter(src, &dest->reset_syserr_conf ); gnd::conf::get_parameter(src, &dest->random_sampling); gnd::conf::get_parameter(src, &dest->pws_id); gnd::conf::get_parameter(src, &dest->ssm_id); gnd::conf::get_parameter(src, &dest->gyro); gnd::conf::get_parameter(src, &dest->gyro_vol); gnd::conf::get_parameter(src, &dest->gyro_bits); gnd::conf::get_parameter(src, &dest->gyro_bias); gnd::conf::get_parameter(src, &dest->gyro_sf); proc_conf_sampling_ratio_normalize(dest); configure_get_covariance(dest); return 0; }
// configure file data analyze inline int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) { gnd_assert(!src, -1, "invalid null pointer"); gnd_assert(!dest, -1, "invalid null pointer"); gnd::conf::set_parameter(dest, &src->kfile); gnd::conf::set_parameter(dest, &src->k_rwheel); gnd::conf::set_parameter(dest, &src->k_rwheel_crot); gnd::conf::set_parameter(dest, &src->k_lwheel); gnd::conf::set_parameter(dest, &src->k_lwheel_crot); gnd::conf::set_parameter(dest, &src->k_tread); gnd::conf::set_parameter(dest, &src->k_gear); gnd::conf::set_parameter(dest, &src->k_encoder); gnd::conf::set_parameter(dest, &src->particles); src->pos_err_ini.value[2] = gnd_ang2deg(src->pos_err_ini.value[2]); gnd::conf::set_parameter(dest, &src->pos_err_ini); src->pos_err_ini.value[2] = gnd_deg2ang(src->pos_err_ini.value[2]); gnd::conf::set_parameter(dest, &src->sys_err_ini); gnd::conf::set_parameter(dest, &src->resmp_rate_remain); gnd::conf::set_parameter(dest, &src->resmp_rate_randerr); src->randerr_conf.value[2] = gnd_ang2deg(src->randerr_conf.value[2]); gnd::conf::set_parameter(dest, &src->randerr_conf); src->randerr_conf.value[2] = gnd_deg2ang(src->randerr_conf.value[2]); src->randerr_offset_conf.value[2] = gnd_ang2deg(src->randerr_offset_conf.value[2]); gnd::conf::set_parameter(dest, &src->randerr_offset_conf); src->randerr_offset_conf.value[2] = gnd_deg2ang(src->randerr_offset_conf.value[2]); gnd::conf::set_parameter(dest, &src->resmp_rate_syserr); gnd::conf::set_parameter(dest, &src->syserr_conf); gnd::conf::set_parameter(dest, &src->resmp_rate_syserr2); gnd::conf::set_parameter(dest, &src->syserr2_conf); gnd::conf::set_parameter(dest, &src->resmp_rate_resetsyserr); gnd::conf::set_parameter(dest, &src->reset_syserr_conf); gnd::conf::set_parameter(dest, &src->random_sampling); gnd::conf::set_parameter(dest, &src->pws_id); gnd::conf::set_parameter(dest, &src->ssm_id); gnd::conf::set_parameter(dest, &src->gyro); gnd::conf::set_parameter(dest, &src->gyro_vol); gnd::conf::set_parameter(dest, &src->gyro_bits); gnd::conf::set_parameter(dest, &src->gyro_bias); gnd::conf::set_parameter(dest, &src->gyro_sf); return 0; }
inline int proc_conf_sampling_ratio_normalize(proc_configuration *conf) { gnd_assert(!conf, -1, "invalid null pointer"); { double sum = 0; sum += conf->resmp_rate_remain.value; sum += conf->resmp_rate_randerr.value; sum += conf->resmp_rate_syserr.value; sum += conf->resmp_rate_syserr2.value; sum += conf->resmp_rate_resetsyserr.value; if( sum <= 0 ){ return -1; } else { conf->resmp_rate_remain.value /= sum; conf->resmp_rate_randerr.value /= sum; conf->resmp_rate_syserr.value /= sum; conf->resmp_rate_syserr2.value /= sum; conf->resmp_rate_resetsyserr.value /= sum; } } return 0; }
/** * @brief initialize process configuration structure * @param [in/out] conf : initialized */ inline int proc_conf_initialize(proc_configuration *conf) { gnd_assert(!conf, -1, "invalid null pointer"); ::memcpy(&conf->bmp_map, &ConfIni_BMPMap, sizeof(ConfIni_BMPMap)); ::memcpy(&conf->raw_map, &ConfIni_RawMap, sizeof(ConfIni_RawMap)); ::memcpy(&conf->sokuikiraw_name, &ConfIni_SokuikiRawName, sizeof(ConfIni_SokuikiRawName) ); ::memcpy(&conf->sokuikiraw_id, &ConfIni_SokuikiRawID, sizeof(ConfIni_SokuikiRawID) ); ::memcpy(&conf->sokuikifs_name, &ConfIni_SokuikiFSName, sizeof(ConfIni_SokuikiFSName) ); ::memcpy(&conf->sokuikifs_id, &ConfIni_SokuikiFSID, sizeof(ConfIni_SokuikiFSID) ); ::memcpy(&conf->odometry_name, &ConfIni_OdometryName, sizeof(ConfIni_OdometryName) ); ::memcpy(&conf->odometry_id, &ConfIni_OdometryID, sizeof(ConfIni_OdometryID) ); ::memcpy(&conf->particle_name, &ConfIni_ParticleName, sizeof(ConfIni_ParticleName) ); ::memcpy(&conf->particle_id, &ConfIni_ParticleID, sizeof(ConfIni_ParticleID) ); ::memcpy(&conf->eval_name, &ConfIni_ParticleEvalName, sizeof(ConfIni_ParticleEvalName) ); ::memcpy(&conf->eval_id, &ConfIni_ParticleEvalID, sizeof(ConfIni_ParticleEvalID) ); ::memcpy(&conf->cycle, &ConfIni_Cycle, sizeof(ConfIni_Cycle)); ::memcpy(&conf->sleeping_time, &ConfIni_SleepingTime, sizeof(ConfIni_SleepingTime)); ::memcpy(&conf->sleeping_dist, &ConfIni_SleepingDist, sizeof(ConfIni_SleepingDist)); ::memcpy(&conf->sleeping_orient, &ConfIni_SleepingOrient, sizeof(ConfIni_SleepingOrient)); ::memcpy(&conf->cull, &ConfIni_Cull, sizeof(ConfIni_Cull)); ::memcpy(&conf->blur, &ConfIni_Blur, sizeof(ConfIni_Blur)); ::memcpy(&conf->scan_range, &ConfIni_ScanRangeDist, sizeof(ConfIni_ScanRangeDist)); ::memcpy(&conf->matching_false, &ConfIni_MatchingFailureRate, sizeof(ConfIni_MatchingFailureRate)); ::memcpy(&conf->false_positive, &ConfIni_MatchingFalsePositive, sizeof(ConfIni_MatchingFalsePositive)); return 0; }
/*! * @brief initialize configure */ inline int proc_conf_initialize(proc_configuration *conf) { gnd_assert(!conf, -1, "invalid null pointer"); ::memcpy(&conf->kfile, &ConfIni_KFile, sizeof(ConfIni_KFile) ); ::memcpy(&conf->output_ssmname, &ConfIni_SSMName, sizeof(ConfIni_SSMName) ); ::memcpy(&conf->output_ssmid, &ConfIni_SSMID, sizeof(ConfIni_SSMID) ); ::memcpy(&conf->odmerr_name, &ConfIni_OdometryErrSSMLogFile, sizeof(ConfIni_OdometryErrSSMLogFile) ); ::memcpy(&conf->odmerr_id, &ConfIni_OdometryErrID, sizeof(ConfIni_OdometryErrID) ); ::memcpy(&conf->ad_ssmname, &ConfIni_ADSSMLogFile, sizeof(ConfIni_ADSSMLogFile) ); ::memcpy(&conf->ad_ssmid, &ConfIni_ADSSMID, sizeof(ConfIni_ADSSMID) ); ::memcpy(&conf->ad_bits, &ConfIni_ADBits, sizeof(ConfIni_ADBits) ); ::memcpy(&conf->ratio_port, &ConfIni_RatioPort, sizeof(ConfIni_RatioPort) ); ::memcpy(&conf->voltage, &ConfIni_Voltage, sizeof(ConfIni_Voltage) ); ::memcpy(&conf->bias, &ConfIni_Bias, sizeof(ConfIni_Bias) ); ::memcpy(&conf->scale_factor, &ConfIni_ScaleFactor, sizeof(ConfIni_ScaleFactor) ); ::memcpy(&conf->motor_ssmname, &ConfIni_MotorSSMName, sizeof(ConfIni_MotorSSMName) ); ::memcpy(&conf->motor_ssmid, &ConfIni_MotorSSMID, sizeof(ConfIni_MotorSSMID) ); ::memcpy(&conf->spur_adjust, &ConfIni_SpurAdjust, sizeof(ConfIni_SpurAdjust) ); ::memcpy(&conf->gyrodometry, &ConfIni_Gyrodometry, sizeof(ConfIni_Gyrodometry) ); ::memcpy(&conf->error_simulation, &ConfIni_ErrorSimulation, sizeof(ConfIni_ErrorSimulation) ); ::memcpy(&conf->error_distributuion, &ConfIni_ErrorDistribution, sizeof(ConfIni_ErrorDistribution) ); ::memcpy(&conf->debug, &ConfIni_Debug, sizeof(ConfIni_Debug) ); return 0; }
inline int queue<T>::insert(const uint64_t i, const T* src, const uint64_t n) { gnd_assert(!src, -1, "invalid argument"); gnd_assert(i > _n, -1, "out of buffer"); gnd_error(n == 0, 0, "ineffectual argument"); // reallocate while(_nalloc < _n + n) __reallocate__(_n + n); // move if(i != _n) __move__(_data + i + n, _data + i, (_n - i)); // copy __copy__(_data + i, src, n); _n += n; return (int)i; }
/* * @brief set configuration parameter */ inline int proc_conf_set(gnd::conf::configuration *dest, proc_configuration *src) { gnd_assert(!dest, -1, "invalid null pointer"); gnd_assert(!src, -1, "invalid null pointer"); gnd::conf::set_parameter(dest, &src->raw_name); gnd::conf::set_parameter(dest, &src->raw_id); gnd::conf::set_parameter(dest, &src->fs_name); gnd::conf::set_parameter(dest, &src->fs_id); gnd::conf::set_parameter(dest, &src->fs_storage); gnd::conf::set_parameter(dest, &src->gl_name); gnd::conf::set_parameter(dest, &src->gl_id); gnd::conf::set_parameter(dest, &src->gl_pos_name); gnd::conf::set_parameter(dest, &src->gl_pos_id); gnd::conf::set_parameter(dest, &src->gl_storage); gnd::conf::set_parameter(dest, &src->gl_txtlog); return 0; }
/** * @breif erase comment paragraph * @param[out] str : string */ inline int file_stream::erase_comment(char *str) { char *p; gnd_assert(!str, -1, "invalid null pointer"); if( !(p = ::strchr(str, TokenComment)) ) return 0; *p = '\0'; return 0; }
/** * @breif erase non-blank character from str head * @param[in/out] str */ inline int file_stream::erase_nonblank(char *str) { int i; gnd_assert(!str, -1, "invalid null pointer"); for(i = 0; str[i] != '\0' && !::isspace(str[i]); i++); ::memmove(str, str + i, ::strlen(str + i) + 1); return i; }
inline int queue<T>::copy(const T* src, const uint64_t n) { gnd_assert(!src, -1, "invalid arugment"); gnd_error(n == 0, 0, "ineffectual argument"); // reallocate while(_nalloc < _n + n) __reallocate__(_n + n); __copy__(_data, src, n); _n = n; return 0; }
inline int queue<T>::move(const uint64_t i, T* dest, const uint64_t n) { gnd_assert(i + n >= _n, -1, "out of buffer"); gnd_error(n == 0, 0, "ineffectual argument"); // copy if(dest) __copy__(dest, _data + i, n); // move if(i + n != _n) __move__(_data + i, _data + i + n, (_n - i - n)); _n -= n; return 0; }
/*! * @brief initialize configure */ inline int proc_conf_initialize(proc_configuration *conf){ gnd_assert(!conf, -1, "invalid null pointer"); ::memcpy(&conf->raw_name, &ConfIni_RawSSMName, sizeof(ConfIni_RawSSMName) ); ::memcpy(&conf->raw_id, &ConfIni_RawSSMID, sizeof(ConfIni_RawSSMID) ); ::memcpy(&conf->fs_name, &ConfIni_FSSSMName, sizeof(ConfIni_FSSSMName) ); ::memcpy(&conf->fs_id, &ConfIni_FSSSMID, sizeof(ConfIni_FSSSMID) ); ::memcpy(&conf->fs_storage, &ConfIni_FSStorageLife, sizeof(ConfIni_FSStorageLife) ); ::memcpy(&conf->gl_name, &ConfIni_GLSSMName, sizeof(ConfIni_GLSSMName) ); ::memcpy(&conf->gl_pos_name, &ConfIni_GLPosSSMName, sizeof(ConfIni_GLPosSSMName) ); ::memcpy(&conf->gl_pos_id, &ConfIni_GLPosSSMID, sizeof(ConfIni_GLPosSSMID) ); ::memcpy(&conf->gl_id, &ConfIni_GLSSMID, sizeof(ConfIni_GLSSMID) ); ::memcpy(&conf->gl_storage, &ConfIni_GLStorageLife, sizeof(ConfIni_GLStorageLife) ); ::memcpy(&conf->gl_txtlog, &ConfIni_GLLogFile, sizeof(ConfIni_GLLogFile) ); return 0; }
/*! * @brief initialize configure */ inline int proc_conf_initialize(proc_configuration *conf){ gnd_assert(!conf, -1, "invalid null pointer"); ::memcpy(&conf->kfile, &ConfIni_KFile, sizeof(ConfIni_KFile)); ::memcpy(&conf->k_lwheel, &ConfIni_KLeftWheel, sizeof(ConfIni_KLeftWheel)); ::memcpy(&conf->k_lwheel_crot, &ConfIni_KLeftWheelCRot, sizeof(ConfIni_KLeftWheelCRot)); ::memcpy(&conf->k_rwheel, &ConfIni_KRightWheel, sizeof(ConfIni_KRightWheel)); ::memcpy(&conf->k_rwheel_crot, &ConfIni_KRightWheelCRot, sizeof(ConfIni_KRightWheelCRot)); ::memcpy(&conf->k_tread, &ConfIni_KTread, sizeof(ConfIni_KTread)); ::memcpy(&conf->k_gear, &ConfIni_KGear, sizeof(ConfIni_KGear)); ::memcpy(&conf->k_encoder, &ConfIni_KEncoder, sizeof(ConfIni_KEncoder)); ::memcpy(&conf->particles, &ConfIni_Particles, sizeof(ConfIni_Particles)); ::memcpy(&conf->resmp_rate_remain, &ConfIni_ResampleRate_Remain, sizeof(ConfIni_ResampleRate_Remain)); ::memcpy(&conf->pos_err_ini, &ConfIni_Initial_PositionError, sizeof(ConfIni_Initial_PositionError)); ::memcpy(&conf->sys_err_ini, &ConfIni_Initial_SysErrorParam, sizeof(ConfIni_Initial_SysErrorParam)); ::memcpy(&conf->randerr_conf, &ConfIni_RandomError, sizeof(ConfIni_RandomError)); ::memcpy(&conf->resmp_rate_randerr, &ConfIni_ResampleRate_RandomError, sizeof(ConfIni_ResampleRate_RandomError)); ::memcpy(&conf->randerr_offset_conf, &ConfIni_RandomErrorOffset, sizeof(ConfIni_RandomErrorOffset)); ::memcpy(&conf->resmp_rate_syserr, &ConfIni_ResampleRate_SysError, sizeof(ConfIni_ResampleRate_SysError)); ::memcpy(&conf->syserr_conf, &ConfIni_SysError, sizeof(ConfIni_SysError)); ::memcpy(&conf->resmp_rate_syserr2, &ConfIni_ResampleRate_SysError2, sizeof(ConfIni_ResampleRate_SysError2)); ::memcpy(&conf->syserr2_conf, &ConfIni_SysError2, sizeof(ConfIni_SysError2)); ::memcpy(&conf->resmp_rate_resetsyserr, &ConfIni_ResampleRate_ResetSysError, sizeof(ConfIni_ResampleRate_ResetSysError)); ::memcpy(&conf->reset_syserr_conf, &ConfIni_ResetSysError, sizeof(ConfIni_ResetSysError)); ::memcpy(&conf->random_sampling, &ConfIni_RandomSampling, sizeof(ConfIni_RandomSampling)); ::memcpy(&conf->pws_id, &ConfIni_PWSSSM, sizeof(ConfIni_PWSSSM)); ::memcpy(&conf->ssm_id, &ConfIni_SSMID, sizeof(ConfIni_SSMID)); ::memcpy(&conf->gyro, &ConfIni_Gyro, sizeof(ConfIni_Gyro)); ::memcpy(&conf->gyro_vol, &ConfIni_GyroVoltage, sizeof(ConfIni_GyroVoltage)); ::memcpy(&conf->gyro_bits, &ConfIni_GyroADBits, sizeof(ConfIni_GyroADBits)); ::memcpy(&conf->gyro_bias, &ConfIni_GyroBias, sizeof(ConfIni_GyroBias)); ::memcpy(&conf->gyro_sf, &ConfIni_GyroScaleFactor, sizeof(ConfIni_GyroScaleFactor)); proc_conf_sampling_ratio_normalize(conf); configure_get_covariance(conf); return 0; }
inline int configure_get_covariance(proc_configuration *conf) { gnd_assert(!conf, -1, "invalid null pointer"); gnd::matrix::set_unit( &conf->poserr_cover_ini ); for(size_t i = 0; i < 3; i++) gnd::matrix::set(&conf->poserr_cover_ini, i, i, gnd_square( conf->pos_err_ini.value[i] ) ); gnd::matrix::set_unit( &conf->randerr_covar ); for(size_t i = 0; i < 3; i++) gnd::matrix::set(&conf->randerr_covar, i, i, gnd_square( conf->randerr_conf.value[i] ) ); gnd::matrix::set_unit( &conf->randerr_covar_offset ); for(size_t i = 0; i < 3; i++) gnd::matrix::set(&conf->randerr_covar_offset, i, i, gnd_square( conf->randerr_offset_conf.value[i] ) ); return 0; }
/** * @brief initialize configure */ inline int proc_conf_initialize(proc_configuration *conf){ gnd_assert(!conf, -1, "invalid null pointer"); ::memcpy(&conf->init_opsm_map, &ConfIni_ScanMatchingMapDir, sizeof(ConfIni_ScanMatchingMapDir) ); ::memcpy(&conf->cmap, &ConfIni_CorrectionMapPath, sizeof(ConfIni_CorrectionMapPath) ); ::memcpy(&conf->odm_logname, &ConfIni_OdometryLogName, sizeof(ConfIni_OdometryLogName) ); ::memcpy(&conf->ls_logname, &ConfIni_LaserScannerLogName, sizeof(ConfIni_LaserScannerLogName) ); ::memcpy(&conf->culling, &ConfIni_Culling, sizeof(ConfIni_Culling) ); ::memcpy(&conf->cycle, &ConfIni_Cycle, sizeof(ConfIni_Cycle) ); ::memcpy(&conf->failure_dist, &ConfIni_FailDist, sizeof(ConfIni_FailDist) ); ::memcpy(&conf->failure_orient, &ConfIni_FailOrient, sizeof(ConfIni_FailOrient) ); ::memcpy(&conf->pause_time, &ConfIni_RestCycle, sizeof(ConfIni_RestCycle) ); ::memcpy(&conf->pause_dist, &ConfIni_RestDist, sizeof(ConfIni_RestDist) ); ::memcpy(&conf->pause_orient, &ConfIni_RestOrient, sizeof(ConfIni_RestOrient) ); ::memcpy(&conf->map_update, &ConfIni_SLAM, sizeof(ConfIni_SLAM) ); ::memcpy(&conf->map_update_time, &ConfIni_MapUpdateTime, sizeof(ConfIni_MapUpdateTime) ); ::memcpy(&conf->map_update_dist, &ConfIni_MapUpdateDist, sizeof(ConfIni_MapUpdateDist) ); ::memcpy(&conf->map_update_orient, &ConfIni_MapUpdateOrient, sizeof(ConfIni_MapUpdateOrient) ); ::memcpy(&conf->optimizer, &ConfIni_Optimizer, sizeof(ConfIni_Optimizer) ); ::memcpy(&conf->converge_dist, &ConfIni_ConvergeDist, sizeof(ConfIni_ConvergeDist) ); ::memcpy(&conf->converge_orient, &ConfIni_ConvergeOrient, sizeof(ConfIni_ConvergeOrient) ); ::memcpy(&conf->ini_map_cnt, &ConfIni_InitMapCnt, sizeof(ConfIni_InitMapCnt) ); ::memcpy(&conf->ini_match_cnt, &ConfIni_InitMatchingCnt, sizeof(ConfIni_InitMatchingCnt) ); ::memcpy(&conf->ndt, &ConfIni_NDT, sizeof(ConfIni_NDT) ); ::memcpy(&conf->cui_show, &ConfIni_DebugShowMode, sizeof(ConfIni_DebugShowMode) ); ::memcpy(&conf->opsm_map, &ConfIni_OPSMMap, sizeof(ConfIni_OPSMMap) ); ::memcpy(&conf->bmp, &ConfIni_BMP, sizeof(ConfIni_BMP) ); ::memcpy(&conf->pos_gridsizex, &ConfIni_PosGridSizeX, sizeof(ConfIni_PosGridSizeX) ); ::memcpy(&conf->pos_gridsizey, &ConfIni_PosGridSizeY, sizeof(ConfIni_PosGridSizeY) ); ::memcpy(&conf->ang_rsl, &ConfIni_AngReslution, sizeof(ConfIni_AngReslution) ); ::memcpy(&conf->trajectory_log, &ConfIni_TrajectoryLog, sizeof(ConfIni_TrajectoryLog) ); ::memcpy(&conf->trajectory4route, &ConfIni_Trajectory4Route, sizeof(ConfIni_Trajectory4Route) ); ::memcpy(&conf->laserpoint_log, &ConfIni_LaserPointLog, sizeof(ConfIni_LaserPointLog) ); ::memcpy(&conf->output_dir, &ConfIni_OutputDir, sizeof(ConfIni_OutputDir) ); ::memcpy(&conf->debug_odo_err_map, &ConfIni_DebugOdometryErrorMap, sizeof(ConfIni_DebugOdometryErrorMap) ); return 0; }
/** * @brief file out configure file */ inline int proc_conf_set( gnd::conf::configuration* dest, proc_configuration* src ) { gnd_assert(!dest, -1, "invalid null pointer"); gnd_assert(!src, -1, "invalid null pointer"); { // ---> operation gnd::conf::set_parameter(dest, &src->odm_logname); gnd::conf::set_parameter(dest, &src->ls_logname); // scan matching parameter gnd::conf::set_parameter(dest, &src->cycle); gnd::conf::set_parameter(dest, &src->culling); gnd::conf::set_parameter(dest, &src->optimizer); gnd::conf::set_parameter(dest, &src->converge_dist); src->converge_orient.value = gnd_ang2deg(src->converge_orient.value); gnd::conf::set_parameter(dest, &src->converge_orient); src->converge_orient.value = gnd_deg2ang(src->converge_orient.value); gnd::conf::set_parameter(dest, &src->failure_dist); src->failure_orient.value = gnd_ang2deg(src->failure_orient.value); gnd::conf::set_parameter(dest, &src->failure_orient); src->failure_orient.value = gnd_deg2ang(src->failure_orient.value); gnd::conf::set_parameter(dest, &src->pause_time); gnd::conf::set_parameter(dest, &src->pause_dist); src->pause_orient.value = gnd_ang2deg(src->pause_orient.value); gnd::conf::set_parameter(dest, &src->pause_orient ); src->pause_orient.value = gnd_deg2ang(src->pause_orient.value); gnd::conf::set_parameter(dest, &src->ini_map_cnt ); gnd::conf::set_parameter(dest, &src->ini_match_cnt ); gnd::conf::set_parameter(dest, &src->ndt); gnd::conf::set_parameter(dest, &src->map_update); gnd::conf::set_parameter(dest, &src->map_update_time); gnd::conf::set_parameter(dest, &src->map_update_dist); src->map_update_orient.value = gnd_ang2deg(src->map_update_orient.value); gnd::conf::set_parameter(dest, &src->map_update_orient); src->map_update_orient.value = gnd_deg2ang(src->map_update_orient.value); gnd::conf::set_parameter(dest, &src->output_dir ); gnd::conf::set_parameter(dest, &src->opsm_map ); gnd::conf::set_parameter(dest, &src->bmp ); gnd::conf::set_parameter(dest, &src->pos_gridsizex ); gnd::conf::set_parameter(dest, &src->pos_gridsizey ); gnd::conf::set_parameter(dest, &src->ang_rsl ); gnd::conf::set_parameter(dest, &src->trajectory_log ); gnd::conf::set_parameter(dest, &src->trajectory4route ); gnd::conf::set_parameter(dest, &src->laserpoint_log ); gnd::conf::set_parameter(dest, &src->cui_show ); gnd::conf::set_parameter(dest, &src->init_opsm_map); gnd::conf::set_parameter(dest, &src->cmap ); return 0; } // <--- operation }
inline bool options::get_option(int aArgc, char **aArgv) { gnd_assert(!param, -1, "parameter storage is null."); while(1){ int opt; optarg = 0; opt = ::getopt_long(aArgc, aArgv, ShortOpt, LongOpt, 0); if(opt < 0) break; switch(opt){ // read configure case 'g': { gnd::conf::file_stream conf_fs; if( conf_fs.read(optarg) < 0 ){ ::fprintf(stderr, " ... [\x1b[1m\x1b[31mERROR\x1b[30m\x1b[0m]: -g option, Fail to read configure file\n"); return RFail; } if( proc_conf_get(&conf_fs, param) < 0){ ::fprintf(stderr, " ... [\x1b[1m\x1b[31mERROR\x1b[30m\x1b[0m]: -g option, configure file syntax error\n"); return RFail; } } break; // write configure case 'G': { proc_conf_write( optarg ? optarg : "ls-coordinate-converter.conf", param); ::fprintf(stdout, " ... output configuration file \"\x1b[4m%s\x1b[0m\"\n", optarg ? optarg : "ls-coordinate-converter.conf"); } return RWriteConf; // write configure // show help case 'h': { int i = 0; fprintf(stderr, "\t\x1b[1mNAME\x1b[0m\n"); fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m - laser scan coordinate convert \n", proc_name); fprintf(stderr, "\n"); fprintf(stderr, "\t\x1b[1mSYNAPSIS\x1b[0m\n"); fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m [\x1b[4mOPTIONS\x1b[0m]\n", proc_name); fprintf(stderr, "\n"); fprintf(stderr, "\t\x1b[1mDISCRIPTION\x1b[0m\n"); fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m convert the coordinate of laser scan.\n", proc_name); fprintf(stderr, "\n"); fprintf(stderr, "\t\x1b[1mOPTIONS\x1b[0m\n"); fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name); fprintf(stderr, "\t\t\tprint help\n"); fprintf(stderr, "\n"); i++; fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name); fprintf(stderr, "\t\t\tread configure file\n"); fprintf(stderr, "\n"); i++; fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name); fprintf(stderr, "\t\t\twirte configure file\n"); fprintf(stderr, "\n"); i++; return RHelp; }break; } } return 0; }