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 で、初期座標をセットするためのフラグです。1度だけ実行します。
  • 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_) {
      初期化。初期位置情報を粒子に摘要。1度しか実行しません。
  • 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の変数にも書き込む。

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*