1+ #ifndef LOCALIZER_HPP
2+ #define LOCALIZER_HPP
3+
4+ #include < rclcpp/rclcpp.hpp>
5+ #include < functional> // bind, placeholders
6+ #include < memory> // SharedPtr
7+ #include < type_traits>
8+ #include < utility>
9+ #include < nav_msgs/msg/occupancy_grid.hpp>
10+ #include < nav_msgs/msg/odometry.hpp>
11+ #include < sensor_msgs/msg/laser_scan.hpp>
12+ #include < geometry_msgs/msg/pose.hpp>
13+ #include < geometry_msgs/msg/pose_stamped.hpp>
14+ #include < geometry_msgs/msg/pose_array.hpp>
15+ #include < tf2/utils.h>
16+ #include < tf2_ros/transform_broadcaster.h>
17+ #include < tf2_geometry_msgs/tf2_geometry_msgs.hpp>
18+
19+ #include < random>
20+
21+ #include " localizer/odom_model.hpp"
22+ #include " localizer/particle.hpp"
23+ #include " localizer/pose.hpp"
24+
25+ class Localizer : public rclcpp ::Node
26+ {
27+ public:
28+ Localizer (); // デフォルトコンストラクタ
29+ int getOdomFreq (); // 制御周期(hz_)を返す関数
30+ void initialize (); // パーティクルの初期化
31+ void process (); // main文のループ内で実行する関数
32+ private:
33+ // ----- 関数（引数あり）------
34+ // コールバック関数
35+ void map_callback (const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
36+ void odom_callback (const nav_msgs::msg::Odometry::SharedPtr msg);
37+ void laser_callback (const sensor_msgs::msg::LaserScan::SharedPtr msg);
38+
39+ // その他の関数
40+ double normalize_angle (double angle); // 適切な角度(-M_PI ~ M_PI)を返す
41+ double norm_rv (const double mean, const double stddev); // ランダム変数生成関数（正規分布）
42+ void resampling (const double alpha); // リサンプリング（系統サンプリング）
43+
44+ // ----- 関数（引数なし）------
45+ void reset_weight (); // パーティクルの重みの初期化
46+ void broadcast_odom_state (); // map座標系からみたodom座標系の位置と姿勢をtfでbroadcast
47+ void localize (); // 自己位置推定
48+ void motion_update (); // 動作更新
49+ void observation_update (); // 観測更新
50+ void estimate_pose (); // 推定位置の決定
51+ void normalize_belief (); // 重みの正規化
52+ void expansion_resetting (); // 膨張リセット
53+ void publish_estimated_pose (); // 推定位置のパブリッシュ
54+ void publish_particles (); // パーティクルクラウドのパブリッシュ
55+ double calc_marginal_likelihood (); // 周辺尤度の算出
56+
57+ // Subscriber
58+ rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_map_;
59+ rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
60+ rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_laser_;
61+
62+ // Publisher
63+ rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_estimated_pose_;
64+ rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_particle_cloud_;
65+
66+ // ----- 変数 -----
67+ // 基本設定
68+ int hz_; // ループ周波数 [Hz]
69+ int particle_num_; // パーティクルの個数 [-]
70+ int max_particle_num_; // パーティクルの個数 [-]
71+ int min_particle_num_; // パーティクルの個数 [-]
72+ double move_dist_th_; // ロボットの移動開始判断用（スタートからの距離の閾値）[m] int reset_counter = 0; // 連続リセットの回数 [-]
73+ // 初期ポーズ関連
74+ double init_x_; // 初期位置 [m]
75+ double init_y_; // 初期位置 [m]
76+ double init_yaw_; // 初期姿勢 [rad]
77+ double init_x_dev_; // 初期位置xの標準偏差 [m]
78+ double init_y_dev_; // 初期位置yの標準偏差 [m]
79+ double init_yaw_dev_; // 初期姿勢の標準偏差 [rad]
80+ // リセット関連
81+ double alpha_th_; // リセットに関する平均尤度の閾値 [-]
82+ int reset_counter = 0 ; // 連続リセットの回数 [-]
83+ int reset_count_limit_; // 連続リセットの回数の上限 [-]
84+ double expansion_x_dev_; // 膨張リセットの位置xの標準偏差 [m]
85+ double expansion_y_dev_; // 膨張リセットの位置yの標準偏差 [m]
86+ double expansion_yaw_dev_; // 膨張リセットの姿勢の標準偏差 [rad]
87+ // センサ関連
88+ int laser_step_; // 何本ずつレーザを見るか [-]
89+ double sensor_noise_ratio_; // 距離に対するセンサノイズ比 [-]
90+
91+ Pose estimated_pose_; // 推定位置
92+ OdomModel odom_model_; // odometryのモデル
93+
94+ // リスト
95+ std::vector<Particle> particles_; // パーティクルクラウド（計算用）
96+ std::vector<double > ignore_angle_range_list_; // 柱に関する角度範囲の配列 [rad]
97+
98+ // msg受け取りフラグ
99+ bool flag_map_ = false ;
100+ bool flag_odom_ = false ;
101+ bool flag_laser_ = false ;
102+
103+ // その他のフラグ
104+ bool flag_move_ = false ; // 機体が動いたか判断用
105+ bool flag_init_noise_; // 初期位置にノイズを加えるか
106+ bool flag_broadcast_; // tf broadcastをするか
107+ bool flag_reverse_; // 初期姿勢を逆にするか
108+ bool is_visible_; // パーティクルクラウドをパブリッシュするか
109+
110+ // OdomModel関連
111+ double ff_; // 直進1[m]で生じる道のりのばらつきの標準偏差 [m]
112+ double fr_; // 回転1[rad]で生じる道のりのばらつきの標準偏差 [m]
113+ double rf_; // 直進1[m]で生じるロボットの向きのばらつきの標準偏差 [rad]
114+ double rr_; // 回転1[rad]で生じるロボットの向きのばらつきの標準偏差 [rad]
115+
116+ // 正規分布用乱数
117+ std::random_device seed_gen_;
118+ std::default_random_engine engine_;
119+
120+ // 各種オブジェクト
121+ nav_msgs::msg::OccupancyGrid map_; // map_serverから受け取るマップ
122+ nav_msgs::msg::Odometry last_odom_; // 最新のodometry
123+ nav_msgs::msg::Odometry prev_odom_; // 1制御周期前のodometry
124+ sensor_msgs::msg::LaserScan laser_; // レーザ値
125+ geometry_msgs::msg::PoseStamped estimated_pose_msg_; // 推定位置
126+ geometry_msgs::msg::PoseArray particle_cloud_msg_; // パーティクルクラウド（パブリッシュ用）
127+ };
128+
129+ #endif
0 commit comments