fawkes – amcl

fawkes-robotino/fawkes/src/plugin/amcl についてのまとめ。

• static double normalize(double z);
  角度z[rad]を与えると、[-pi, +pi]の範囲に直した値を戻す。
• static double angle_diff(double a, double b);
  角度a[rad]とb[rad]の差を返します。内角側を返します。
• void AmclThread::init();
  初期化します。read_map_config で、map情報を読み込み、yaml ファイルから、設定をよみこみます。
• bool set_laser_pose();
• bool get_odom_pose(fawkes::tf::Stamped& odom_pose, double& x, double& y, double& yaw, const fawkes::Time* t, const std::string& f);
• void apply_initial_pose();
• static pf_vector_t uniform_pose_generator(void* arg);
• void set_initial_pose(const std::string &frame_id, const fawkes::Time &msg_time, const fawkes::tf::Pose &pose, const double *covariance);
• virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) throw();
• fawkes::Mutex *conf_mutex_;
  スレッド作成時に、Mutex()を代入します。
• map_t* map_;
  スレッド作成時に、NULLを代入します。
• std::string cfg_map_file_;
  read_map_config にて値を設定します。
• float cfg_resolution_;
  read_map_config にて値を設定します。
• float cfg_origin_x_;
  read_map_config にて値を設定します。
• float cfg_origin_y_;
  read_map_config にて値を設定します。
• float cfg_origin_theta_;
  read_map_config にて値を設定します。
• float cfg_occupied_thresh_;
  read_map_config にて値を設定します。
• float cfg_free_thresh_;
  read_map_config にて値を設定します。
• bool cfg_read_init_cov_;
• bool cfg_buffer_enable_;
  yamlファイルのbuffering/enableの項目を読み込みます。
• bool cfg_buffer_debug_;
  yamlファイルのbuffering/debugの項目を読み込みます。
• std::string cfg_laser_ifname_;
  yamlファイルのlaser_interface_idの項目を読み込みます。
• std::string cfg_pose_ifname_;
  yamlファイルのpose_interface_idの項目を読み込みます。
• unsigned int map_width_;
  read_mapで読み込んだ地図情報のxサイズの値です。
• unsigned int map_height_;
  read_mapで読み込んだ地図情報のyサイズの値です。
• bool laser_pose_set_;
  loop で、初期座標をセットするためのフラグです。１度だけ実行します。
• fawkes::Time save_pose_last_time;
  set_clock(clock)で、時刻を記録します。
  stamp()で、何かをします。
• bool sent_first_transform_;
  初期値は、false です。
• bool latest_tf_valid_;
  初期値は、false です。
• int resample_count_;
  初期値は、0 です。
• amcl::AMCLOdom* odom_;
  初期値は、NULL です。
• amcl::AMCLLaser* laser_;
  初期値は、NULL です。
• amcl_hyp_t* initial_pose_hyp_;
  初期値は、NULL です。
• bool first_map_received_;
  初期値は、false です。
• bool first_reconfigure_call_;
  初期値は、false です。
• float init_pose_[3];
  yamlファイルのinit_pose_x、init_pose_y、init_pose_aを読み込みます。設定がない場合は、default値 0.0 です。粒子の初期位置っぽいです。
• float init_cov_[3];
  yamlファイルのread_init_covがtrue だと、init_cov_xx、init_cov_yy、init_cov_aa、を読み込みます。設定がない場合は、default値 0.5*0.5, 0.5*0.5, (M_PI / 12.0) * (M_PI / 12.0) です(PI/12 = 15度)。粒子の分散の値っぽいです。
• double save_pose_period_;
  yamlファイルのsave_pose_period を読み込みます。
• double laser_min_range_;
  yamlファイルのlaser_min_range を読み込みます。
• double laser_max_range_;
  yamlファイルのlaser_max_range を読み込みます。
• double pf_err_, pf_z_;
  yamlファイルのkld_errとkld_z を読み込みます。
• float alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ ;
  yamlファイルのalpha1 から5 を読み込みます。オドメトリのモデルの変数のようです。OMNIの時は1から5を使い、DIFFの時は1から4を使います。
• float z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, laser_likelihood_max_dist_;
  yamlファイルのz_hit、z_short、z_max、z_rand、sigma_hit、lambda_short_、laser_likelihood_max_dist を読み込みます。レーザーの計算時に、BEAMタイプの時と、LikelihoodFieldタイプの時で、使う変数が異なります。
• float d_thresh_;
  yamlファイルのd_thresh を読み込みます。
• float a_thresh_;
  yamlファイルのa_thresh を読み込みます。
• float t_thresh_;
  yamlファイルのt_thresh を読み込みます。
• float alpha_slow_;
  yamlファイルのalpha_slow を読み込みます。
• float alpha_fast_;
  yamlファイルのalpha_fast を読み込みます。
• float angle_increment_;
  yamlファイルのangle_increment を読み込みます。
• unsigned int angle_min_idx_;
  yamlファイルのangle_min_idx [度]を読み込みます（最大値359）。項目がない場合は0にします。
• unsigned int angle_max_idx_;
  yamlファイルのangle_max_idx [度]を読み込みます（最大値359）。項目がない場合は359にします。
• unsigned int angle_range_;
  angle_max_idxとangle_min_idx の差[度]です（内角の大きさ）。
• float angle_min_;
  angle_min_idx　をradian に変換した値です。
• int max_beams_, min_particles_, max_particles_;
  yamlファイルのmax_beams、min_particles、max_particles を読み込みます。
  min_particles_ > max_particles_ だと、警告が出て、max_particles = min_particles にします。
• unsigned int resample_interval_;
  yamlファイルのresample_interval を読み込みます。
• std::string odom_frame_id_;
  yamlファイルの/frames/odom を読み込みます（amclとは項目名が異なるので要注意）。
  同様にbase_frame_id_とglobal_frame_id_にyamlファイルの/frames/base と/frames/fixed を読み込みます（amclとは項目名が異なるので要注意）。
• amcl::laser_model_t laser_model_type_;
  yamlファイルのlaser_model_typeが”beam”の場合、::amcl::LASER_MODEL_BEAM;を代入します。”likelihood_field”の場合、::amcl::LASER_MODEL_LIKELIHOOD_FIELD;を代入します。それ以外の時は，種別不明のため、::amcl::LASER_MODEL_LIKELIHOOD_FIELD;を代入します。
• amcl::odom_model_t odom_model_type_;
  yamlファイルのodom_model_typeが”diff”の場合、 ::amcl::ODOM_MODEL_DIFF;を代入します。”omni”の場合、::amcl::ODOM_MODEL_OMNI;を代入します。それ以外の時は，種別不明のため、::amcl::ODOM_MODEL_DIFF;を代入します。
• double transform_tolerance_;
  yamlファイルのtransform_toleranceを読み込みます。
• pf_t *pf_;
  pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, (pf_init_model_fn_t) AmclThread::uniform_pose_generator, (void *) map_); と、pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator, (void *)map_); で、粒子の初期化をします。
• fawkes::Laser360Interface *laser_if_;
  blackboard のLaser360Interface を読むためです。
• fawkes::Position3DInterface *pos3d_if_;
  blackboard のPosition3DInterface を書くためです。
• fawkes::LocalizationInterface *loc_if_;
  blackboard のLocalizationInterface を書くためです。
• fawkes::tf::Transform latest_tf_;
• amcl::odom_model_t odom_model_type_;
• bool pf_init_;
• pf_vector_t pf_odom_pose_;
• bool laser_update_;
• bool laser_buffered_;
• fawkes::Time last_cloud_pub_time;
• fawkes::Time last_laser_received_ts_;
• double last_covariance_[36];
• fawkes::Time *last_move_time_;

void AmclThread::loop()

• if (!laser_pose_set_) {
  初期化。初期位置情報を粒子に適用。１度しか実行しません。
• if (laser_if_->changed()) {
  レーザーの値が更新されていたら、オドメトリの値を確認して、前回からの移動情報を手に入れます。
• for (int hyp_count = 0; hyp_count < pf_->sets[pf_ – current_set].cluster_count; hyp_count++) {
  粒子の更新をし、重みを計算し、一番重みが大きい粒子を探す。
• if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
  hyp_count番目の粒子の重みと分散の計算。
• if (max_weight > 0.0) {
  一番重みの大きい粒子が、現在の自己位置に近いので、その情報を用いて自己位置を計算。
  その結果を、yamlの変数にも書き込む。