Localizer

a_localizer/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(team_a_localizer)
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# package.xmlのdependをチェックして自動的にfind_packageしてくれる
+ament_auto_find_build_dependencies()
+# 実行ファイルを作成する
+ament_auto_add_executable(${PROJECT_NAME}_node
+  src/localizer.cpp
+  src/localizer_node.cpp
+  src/odom_model.cpp
+  src/particle.cpp
+  src/pose.cpp)
+# launchファイルをインストール（ament_package_indexが探せる場所に置く）
+install(DIRECTORY
+  launch
+  param
+  DESTINATION share/${PROJECT_NAME}/
+)
+# ヘッダーのインストールや共有ライブラリのエクスポート等を自動で行ってくれる
+ament_auto_package()

a_localizer/config/param/localizer.yaml

@@ -0,0 +1,46 @@
+node_name1:
+  ros__parameters:
+
+    # 基本設定
+    hz: ######              # ループ周期 [Hz]
+    particle_num: ######   # パーティクルの個数 [-]
+    max_particle_num: ######   #パーティクルの最大個数
+    min_particle_num: ######   #パーティクルの最小個数
+    move_dist_th: ###### # ロボットの移動開始判断用（スタートからの距離の閾値）[m]
+
+    # 初期ポーズ関連
+    init_x: 0.0       # 初期位置 [m]
+    init_y: 0.0       # 初期位置 [m]
+    init_yaw: 0.0     # 初期姿勢 [rad]
+    init_x_dev: 0.50  # 初期位置xの標準偏差 [m]
+    init_y_dev: 0.65  # 初期位置yの標準偏差 [m]
+    init_yaw_dev: 0.5 # 初期姿勢の標準偏差 [rad]
+
+    # リセット関連
+    alpha_th: 0.0017        # リセットに関する平均尤度の閾値 [-]
+    reset_count_limit: 5    # 連続リセットの回数の上限 [-]
+    expansion_x_dev: 0.05   # 膨張リセットの位置xの標準偏差 [m]
+    expansion_y_dev: 0.05   # 膨張リセットの位置yの標準偏差 [m]
+    expansion_yaw_dev: 0.01 # 膨張リセットの姿勢の標準偏差 [rad]
+
+    # センサ関連
+    laser_step: 10           # 何本ずつレーザを見るか(全1080本)[-]
+    sensor_noise_ratio: 0.03 # 距離に対するセンサノイズ比 [-]
+
+    # --- 柱に関する角度範囲の配列 [rad] ---
+    # 以下が無視される
+    # [0] < angle < [1], [2] < angle
+    # [num] -> num:index
+    ignore_angle_range_list: [#####, #####, #####]
+
+    # フラグ
+    flag_init_noise: true # 初期位置にノイズを加えるか
+    flag_broadcast: true  # tf情報を含むbagファイルを使うときはfalse
+    flag_reverse: false   # 初期姿勢を逆にするか
+    is_visible: true      # パーティクルクラウドを表示（パブリッシュ）するか
+
+    # ===== クラスOdomModel =====
+    ff: 0.17   # 直進1[m]で生じる道のりのばらつきの標準偏差 [m]
+    fr: 0.0005 # 回転1[rad]で生じる道のりのばらつきの標準偏差 [m]
+    rf: 0.13   # 直進1[m]で生じる向きのばらつきの標準偏差 [rad]
+    rr: 0.2    # 回転1[rad]で生じる向きのばらつきの標準偏差 [rad]

a_localizer/include/localizer/localizer.hpp

@@ -0,0 +1,129 @@
+#ifndef LOCALIZER_HPP
+#define LOCALIZER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <functional>   // bind, placeholders
+#include <memory>       // SharedPtr
+#include <type_traits>
+#include <utility>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <tf2/utils.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <random>
+
+#include "localizer/odom_model.hpp"
+#include "localizer/particle.hpp"
+#include "localizer/pose.hpp"
+
+class Localizer : public rclcpp::Node
+{
+    public:
+        Localizer();        // デフォルトコンストラクタ
+        int getOdomFreq();  // 制御周期(hz_)を返す関数
+        void initialize();  // パーティクルの初期化
+        void process();     // main文のループ内で実行する関数
+    private:
+        // ----- 関数（引数あり）------
+        // コールバック関数
+        void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+        void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+        void laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
+        // その他の関数
+        double normalize_angle(double angle);                    // 適切な角度(-M_PI ~ M_PI)を返す
+        double norm_rv(const double mean, const double stddev);  // ランダム変数生成関数（正規分布）
+        void   resampling(const double alpha);                   // リサンプリング（系統サンプリング）
+
+        // ----- 関数（引数なし）------
+        void   reset_weight();             // パーティクルの重みの初期化
+        void   broadcast_odom_state();     // map座標系からみたodom座標系の位置と姿勢をtfでbroadcast
+        void   localize();                 // 自己位置推定
+        void   motion_update();            // 動作更新
+        void   observation_update();       // 観測更新
+        void   estimate_pose();            // 推定位置の決定
+        void   normalize_belief();         // 重みの正規化
+        void   expansion_resetting();      // 膨張リセット
+        void   publish_estimated_pose();   // 推定位置のパブリッシュ
+        void   publish_particles();        // パーティクルクラウドのパブリッシュ
+        double calc_marginal_likelihood(); // 周辺尤度の算出
+
+        // Subscriber
+        rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_map_;
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_laser_;
+
+        // Publisher
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_estimated_pose_;
+        rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_particle_cloud_;
+
+        // ----- 変数 -----
+        // 基本設定
+        int       hz_;                 // ループ周波数 [Hz]
+        int       particle_num_;       // パーティクルの個数 [-]
+        int       max_particle_num_;   // パーティクルの個数 [-]
+        int       min_particle_num_;   // パーティクルの個数 [-]
+        double    move_dist_th_;       // ロボットの移動開始判断用（スタートからの距離の閾値）[m]    int       reset_counter = 0;   // 連続リセットの回数 [-]
+        // 初期ポーズ関連
+        double    init_x_;             // 初期位置 [m]
+        double    init_y_;             // 初期位置 [m]
+        double    init_yaw_;           // 初期姿勢 [rad]
+        double    init_x_dev_;         // 初期位置xの標準偏差 [m]
+        double    init_y_dev_;         // 初期位置yの標準偏差 [m]
+        double    init_yaw_dev_;       // 初期姿勢の標準偏差 [rad]
+        // リセット関連
+        double    alpha_th_;           // リセットに関する平均尤度の閾値 [-]
+        int       reset_counter = 0;   // 連続リセットの回数 [-]
+        int       reset_count_limit_;  // 連続リセットの回数の上限 [-]
+        double    expansion_x_dev_;    // 膨張リセットの位置xの標準偏差 [m]
+        double    expansion_y_dev_;    // 膨張リセットの位置yの標準偏差 [m]
+        double    expansion_yaw_dev_;  // 膨張リセットの姿勢の標準偏差 [rad]
+        // センサ関連
+        int       laser_step_;         // 何本ずつレーザを見るか [-]
+        double    sensor_noise_ratio_; // 距離に対するセンサノイズ比 [-]
+
+        Pose      estimated_pose_;     // 推定位置
+        OdomModel odom_model_;         // odometryのモデル
+
+        // リスト
+        std::vector<Particle> particles_;             // パーティクルクラウド（計算用）
+        std::vector<double> ignore_angle_range_list_; // 柱に関する角度範囲の配列 [rad]
+
+        // msg受け取りフラグ
+        bool flag_map_     = false;
+        bool flag_odom_    = false;
+        bool flag_laser_   = false;
+
+        // その他のフラグ
+        bool flag_move_ = false; // 機体が動いたか判断用
+        bool flag_init_noise_;   // 初期位置にノイズを加えるか
+        bool flag_broadcast_;    // tf broadcastをするか
+        bool flag_reverse_;      // 初期姿勢を逆にするか
+        bool is_visible_;        // パーティクルクラウドをパブリッシュするか
+
+        // OdomModel関連
+        double ff_;     // 直進1[m]で生じる道のりのばらつきの標準偏差 [m]
+        double fr_;     // 回転1[rad]で生じる道のりのばらつきの標準偏差 [m]
+        double rf_;     // 直進1[m]で生じるロボットの向きのばらつきの標準偏差 [rad]
+        double rr_;     // 回転1[rad]で生じるロボットの向きのばらつきの標準偏差 [rad]
+
+        // 正規分布用乱数
+        std::random_device seed_gen_;
+        std::default_random_engine engine_;
+
+        // 各種オブジェクト
+        nav_msgs::msg::OccupancyGrid    map_;                // map_serverから受け取るマップ
+        nav_msgs::msg::Odometry         last_odom_;          // 最新のodometry
+        nav_msgs::msg::Odometry         prev_odom_;          // 1制御周期前のodometry
+        sensor_msgs::msg::LaserScan     laser_;              // レーザ値
+        geometry_msgs::msg::PoseStamped estimated_pose_msg_; // 推定位置
+        geometry_msgs::msg::PoseArray   particle_cloud_msg_; // パーティクルクラウド（パブリッシュ用）
+};
+
+#endif

a_localizer/include/localizer/odom_model.hpp

@@ -0,0 +1,32 @@
+#ifndef ODOM_MODEL_HPP
+#define ODOM_MODEL_HPP
+
+#include <random>
+
+class OdomModel
+{
+    public:
+        OdomModel();    // デフォルトコンストラクタ
+        OdomModel(const double ff, const double fr, const double rf, const double rr);  // コンストラクタ
+        OdomModel& operator =(const OdomModel& model); // 代入演算子
+
+        void   set_dev(const double length, const double angle); // 標準偏差の設定
+        double get_fw_noise();  // 直進に関するノイズの取得
+        double get_rot_noise(); // 回転に関するノイズの取得
+
+    private:
+        double fw_var_per_fw_;   // ffの分散
+        double fw_var_per_rot_;  // frの分散
+        double rot_var_per_fw_;  // rfの分散
+        double rot_var_per_rot_; // rrの分散
+
+        double fw_dev_;  // 直進に関するノイズ
+        double rot_dev_; // 回転に関するノイズ
+
+        // 正規分布
+        std::normal_distribution<> std_norm_dist_;
+        std::random_device seed_gen_;
+        std::default_random_engine engine_;
+};
+
+#endif

a_localizer/include/localizer/particle.hpp

@@ -0,0 +1,47 @@
+#ifndef PARTICLE_HPP
+#define PARTICLE_HPP
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include "localizer/pose.hpp"
+
+class Particle
+{
+    public:
+        Particle(); // デフォルトコンストラクタ
+        Particle(const double x, const double y, const double yaw, const double weight); // コンストラクタ
+        Particle& operator =(const Particle& p); // 代入演算子
+
+        // accessor
+        void set_weight(const double weight);   // 重みのセット
+        double weight() const { return weight_; }
+
+        // 尤度関数
+        double likelihood(const nav_msgs::msg::OccupancyGrid& map, const sensor_msgs::msg::LaserScan& laser,
+            const double sensor_noise_ratio, const int laser_step, const std::vector<double>& ignore_angle_range_list);
+
+        // --- メンバ変数 ---
+        Pose pose_;
+
+    private:
+        // 柱か判断
+        bool is_ignore_angle(double angle, const std::vector<double>& ignore_angle_range_list);
+
+        // 壁までの距離を算出
+        double calc_dist_to_wall(double x, double y, const double laser_angle, const nav_msgs::msg::OccupancyGrid& map,
+            const double laser_range, const double sensor_noise_ratio);
+
+        // 座標からグリッドのインデックスを返す
+        int  xy_to_grid_index(const double x, const double y, const nav_msgs::msg::MapMetaData& map_info);
+
+        // マップ内か判断
+        bool in_map(const int grid_index, const int map_data_size);
+
+        // 確率密度関数（正規分布）
+        double norm_pdf(const double x, const double mean, const double stddev);
+
+        // --- メンバ変数 ---
+        double weight_; // [-]
+};
+
+#endif

a_localizer/include/localizer/pose.hpp

@@ -0,0 +1,32 @@
+#ifndef POSE_HPP
+#define POSE_HPP
+
+#include <cmath>
+
+class Pose
+{
+    public:
+        Pose(); // デフォルトコンストラクタ
+        Pose(const double x, const double y, const double yaw); // コンストラクタ
+        Pose& operator =(const Pose& pose); // 代入演算子
+        Pose& operator /=(const double a);  // 複合代入演算子/=
+
+        // accessor
+        void set(const double x, const double y, const double yaw);
+        double x()   const { return x_; }
+        double y()   const { return y_; }
+        double yaw() const { return yaw_; }
+
+        // パーティクルの移動
+        void move(double length, double direction, double rotation, const double fw_noise, const double rot_noise);
+
+        // 適切な角度(-M_PI ~ M_PI)に変更
+        void normalize_angle();
+
+    // private:
+        double x_;   // [m]
+        double y_;   // [m]
+        double yaw_; // [rad]
+};
+
+#endif

a_localizer/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>team_a_localizer</name>
+  <version>0.0.0</version>
+  <description>the team_a_localizer package</description>
+  <maintainer email="amsl@todo.todo">amsl</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>ament_cmake_auto</depend>
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>random</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>