diff --git a/a_localizer/CMakeLists.txt b/a_localizer/CMakeLists.txt new file mode 100644 index 0000000..5b359bf --- /dev/null +++ b/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() \ No newline at end of file diff --git a/a_localizer/config/param/localizer.yaml b/a_localizer/config/param/localizer.yaml new file mode 100644 index 0000000..b34e71c --- /dev/null +++ b/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] \ No newline at end of file diff --git a/a_localizer/include/localizer/localizer.hpp b/a_localizer/include/localizer/localizer.hpp new file mode 100644 index 0000000..6aa10ac --- /dev/null +++ b/a_localizer/include/localizer/localizer.hpp @@ -0,0 +1,129 @@ +#ifndef LOCALIZER_HPP +#define LOCALIZER_HPP + +#include +#include // bind, placeholders +#include // SharedPtr +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_laser_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_estimated_pose_; + rclcpp::Publisher::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 particles_; // パーティクルクラウド（計算用） + std::vector 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 \ No newline at end of file diff --git a/a_localizer/include/localizer/odom_model.hpp b/a_localizer/include/localizer/odom_model.hpp new file mode 100644 index 0000000..92e2c5c --- /dev/null +++ b/a_localizer/include/localizer/odom_model.hpp @@ -0,0 +1,32 @@ +#ifndef ODOM_MODEL_HPP +#define ODOM_MODEL_HPP + +#include + +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 diff --git a/a_localizer/include/localizer/particle.hpp b/a_localizer/include/localizer/particle.hpp new file mode 100644 index 0000000..0dfc4d4 --- /dev/null +++ b/a_localizer/include/localizer/particle.hpp @@ -0,0 +1,47 @@ +#ifndef PARTICLE_HPP +#define PARTICLE_HPP + +#include +#include +#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& ignore_angle_range_list); + + // --- メンバ変数 --- + Pose pose_; + + private: + // 柱か判断 + bool is_ignore_angle(double angle, const std::vector& 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 diff --git a/a_localizer/include/localizer/pose.hpp b/a_localizer/include/localizer/pose.hpp new file mode 100644 index 0000000..f69c76f --- /dev/null +++ b/a_localizer/include/localizer/pose.hpp @@ -0,0 +1,32 @@ +#ifndef POSE_HPP +#define POSE_HPP + +#include + +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 diff --git a/a_localizer/package.xml b/a_localizer/package.xml new file mode 100644 index 0000000..25ec7f8 --- /dev/null +++ b/a_localizer/package.xml @@ -0,0 +1,24 @@ + + + team_a_localizer + 0.0.0 + the team_a_localizer package + amsl + TODO + + ament_cmake_ros + + ament_cmake_auto + rclcpp + nav_msgs + sensor_msgs + geometry_msgs + random + tf2 + tf2_ros + tf2_geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/a_localizer/src/localizer.cpp b/a_localizer/src/localizer.cpp new file mode 100644 index 0000000..ab1fa8f --- /dev/null +++ b/a_localizer/src/localizer.cpp @@ -0,0 +1,434 @@ +#include "localizer/localizer.hpp" + +// デフォルトコンストラクタ +// パラメータの宣言と取得 +// Subscriber，Publisherの設定 +// frame idの設定 +// パーティクルクラウドのメモリの確保 +// odometryのモデルの初期化 +Localizer::Localizer() : Node("team_localizer") +{ + // パラメータの宣言 + this->declare_parameter("hz", 10); + this->declare_parameter("particle_num", 500); + this->declare_parameter("max_particle_num", 1000); + this->declare_parameter("min_particle_num", 100); + this->declare_parameter("move_dist_th", 0.2); + this->declare_parameter("init_x", 0.0); + this->declare_parameter("init_y", 0.0); + this->declare_parameter("init_yaw", 0.0); + this->declare_parameter("init_x_dev", 0.50); + this->declare_parameter("init_y_dev", 0.65); + this->declare_parameter("init_yaw_dev", 0.50); + this->declare_parameter("alpha_th", 0.0017); + this->declare_parameter("reset_count_limit", 5); + this->declare_parameter("expansion_x_dev", 0.05); + this->declare_parameter("expansion_y_dev", 0.05); + this->declare_parameter("expansion_yaw_dev", 0.1); + this->declare_parameter("laser_step", 10); + this->declare_parameter("sensor_noise_ratio", 0.03); + this->declare_parameter("ff", 0.17); + this->declare_parameter("fr", 0.0005); + this->declare_parameter("rf", 0.13); + this->declare_parameter("rr", 0.2); + // this->declare_parameter("ignore_angle_range_list", std::vector({M_PI*1.5/16.0, M_PI*5.0/16.0, M_PI*10.0/16.0})); + + // パラメータの取得 + this->get_parameter("hz", hz_); + this->get_parameter("particle_num", particle_num_); + this->get_parameter("max_particle_num", max_particle_num_); + this->get_parameter("min_particle_num", min_particle_num_); + this->get_parameter("move_dist_th", move_dist_th_); + this->get_parameter("init_x", init_x_); + this->get_parameter("init_y", init_y_); + this->get_parameter("init_yaw", init_yaw_); + this->get_parameter("init_x_dev", init_x_dev_); + this->get_parameter("init_y_dev", init_y_dev_); + this->get_parameter("init_yaw_dev", init_yaw_dev_); + this->get_parameter("alpha_th", alpha_th_); + this->get_parameter("reset_count_limit", reset_count_limit_); + this->get_parameter("expansion_x_dev", expansion_x_dev_); + this->get_parameter("expansion_y_dev", expansion_y_dev_); + this->get_parameter("expansion_yaw_dev", expansion_yaw_dev_); + this->get_parameter("laser_step", laser_step_); + this->get_parameter("sensor_noise_ratio", sensor_noise_ratio_); + this->get_parameter("ff", ff_); + this->get_parameter("fr", fr_); + this->get_parameter("rf", rf_); + this->get_parameter("rr", rr_); + // this->get_parameter("ignore_angle_range_list", ignore_angle_range_list_); + + // Subscriberの設定 + sub_map_ = this->create_subscription( + "/map", rclcpp::QoS(10).reliable(), std::bind( + &Localizer::map_callback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription( + "/odom", rclcpp::QoS(10).reliable(), std::bind( + &Localizer::odom_callback, this, std::placeholders::_1)); + sub_laser_ = this->create_subscription( + "/scan", rclcpp::QoS(10).reliable(), std::bind( + &Localizer::laser_callback, this, std::placeholders::_1)); + + // Publisherの設定 + pub_estimated_pose_ = this->create_publisher( + "/estimated_pose", rclcpp::QoS(10).reliable()); + pub_particle_cloud_ = this->create_publisher( + "/particle_cloud", rclcpp::QoS(10).reliable()); + + // frame idの設定 + estimated_pose_msg_.header.frame_id = "map"; + particle_cloud_msg_.header.frame_id = "map"; + + // パーティクルクラウドのメモリの確保 + particle_cloud_msg_.poses.reserve(max_particle_num_); + + // odometryのモデルの初期化 + OdomModel odom_model_(ff_, fr_, rf_, rr_); +} + +// mapのコールバック関数 +void Localizer::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + map_ = *msg; + flag_map_ = true; + printf("map call ok\n"); +} + +// odometryのコールバック関数 +void Localizer::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + prev_odom_ = last_odom_; + last_odom_ = *msg; + flag_odom_ = true; + // printf("%f -> %f\n", prev_odom_.pose.pose.position.x, last_odom_.pose.pose.position.x); +} + +// laserのコールバック関数 +void Localizer::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) +{ + laser_ = *msg; + flag_laser_ = true; + // printf("laser call ok\n"); +} + +// hz_を返す関数 +int Localizer::getOdomFreq() +{ + return hz_; +} + +// ロボットとパーティクルの推定位置の初期化 +void Localizer::initialize() +{ + // 推定位置の初期化 + estimated_pose_.set(init_x_, init_y_, init_yaw_); + + Particle particle; + + // 初期位置近傍にパーティクルを配置 + for(int i=0; i norm_dist(mean, stddev); + return norm_dist(engine_); +} + +// パーティクルの重みの初期化 +void Localizer::reset_weight() +{ + for(auto& p : particles_){ + p.set_weight(1.0/particles_.size()); + } +} + +// map座標系からみたodom座標系の位置と姿勢をtfでbroadcast +// map座標系からみたbase_link座標系の位置と姿勢，odom座標系からみたbase_link座標系の位置と姿勢から計算 +void Localizer::broadcast_odom_state() +{ + flag_broadcast_ = true; + if(flag_broadcast_) + { + // TF Broadcasterの実体化 + static std::shared_ptr odom_state_broadcaster; + // broadcasterの初期化 + odom_state_broadcaster = std::make_shared(this); + + // map座標系からみたbase_link座標系の位置と姿勢の取得 + Pose map_to_base = estimated_pose_; + + // odom座標系からみたbase_link座標系の位置と姿勢の取得 + Pose odom_to_base; + odom_to_base.x_ = last_odom_.pose.pose.position.x; + odom_to_base.y_ = last_odom_.pose.pose.position.y; + odom_to_base.yaw_ = tf2::getYaw(last_odom_.pose.pose.orientation); + + // map座標系からみたodom座標系の位置と姿勢を計算（回転行列を使った単純な座標変換） + const double map_to_odom_x = map_to_base.x() - odom_to_base.x(); + const double map_to_odom_y = map_to_base.y() - odom_to_base.y(); + const double map_to_odom_yaw = map_to_base.yaw() - odom_to_base.yaw(); + + // yawからquaternionを作成 + tf2::Quaternion map_to_odom_quat; + map_to_odom_quat.setRPY(0, 0, map_to_odom_yaw); + + // odom座標系odomの位置姿勢情報格納するための変数 + geometry_msgs::msg::TransformStamped odom_state; + + // 現在の時間の格納 + odom_state.header.stamp = get_clock()->now(); + + // 親フレーム・子フレームの指定 + odom_state.header.frame_id = map_.header.frame_id; + odom_state.child_frame_id = last_odom_.header.frame_id; + + // map座標系からみたodom座標系の原点位置と方向の格納 + odom_state.transform.translation.x = map_to_odom_x; + odom_state.transform.translation.y = map_to_odom_y; + odom_state.transform.rotation.x = map_to_odom_quat.x(); + odom_state.transform.rotation.y = map_to_odom_quat.y(); + odom_state.transform.rotation.z = map_to_odom_quat.z(); + odom_state.transform.rotation.w = map_to_odom_quat.w(); + + // tf情報をbroadcast(座標系の設定) + odom_state_broadcaster->sendTransform(odom_state); + + // printf("broadcast ok\n"); + } + +} + +// 自己位置推定 +// 動作更新と観測更新を行う +void Localizer::localize() +{ + motion_update(); + observation_update(); +} + +// 動作更新 +// ロボットの微小移動量を計算し，パーティクルの位置をノイズを加えて更新 +void Localizer::motion_update() +{ + // 微小移動量の計算 + const double prev_yaw = tf2::getYaw(prev_odom_.pose.pose.orientation); + const double last_yaw = tf2::getYaw(last_odom_.pose.pose.orientation); + const double dx = last_odom_.pose.pose.position.x - prev_odom_.pose.pose.position.x; + const double dy = last_odom_.pose.pose.position.y - prev_odom_.pose.pose.position.y; + const double dyaw = normalize_angle(last_yaw - prev_yaw); + + // 方向、移動量の計算 + const double length = hypot(dx, dy); + const double direction = normalize_angle(atan2(dy, dx) - prev_yaw); + // printf("%f, %f\n", length, direction); + + // 標準偏差の設定 + odom_model_.set_dev(length, dyaw); + + // パーティクルの位置更新 + for(auto& p : particles_){ + p.pose_.move(length, direction, dyaw, odom_model_.get_fw_noise(), odom_model_.get_rot_noise()); + } + // printf("mu ok\n"); +} + +// 観測更新 +// パーティクルの尤度を計算し，重みを更新 +// 位置推定，リサンプリングなどを行う +void Localizer::observation_update() +{ + for(auto& p : particles_){ + p.set_weight(p.weight() * p.likelihood(map_, laser_, sensor_noise_ratio_, laser_step_, ignore_angle_range_list_)); + } + // パーティクル1つのレーザ1本における平均尤度を算出 + const double alpha = calc_marginal_likelihood()/((laser_.ranges.size()/laser_step_) * particles_.size()); + // printf("alpha %f\n", alpha); + + normalize_belief(); + resampling(alpha); + estimate_pose(); + // printf("ou ok\n"); +} + +// 周辺尤度の算出 +double Localizer::calc_marginal_likelihood() +{ + double sum = 0.0; + for(const auto& p : particles_){ + sum += p.weight(); + } + // printf("sum %f\n", sum); + return sum; +} + +// 推定位置の決定 +// 算出方法は複数ある（平均，加重平均，中央値など...） +void Localizer::estimate_pose() +{ + double estimate_x = 0.0; + double estimate_y = 0.0; + double estimate_yaw = 0.0; + double max_weight = 0.0; + // printf("weight %f\n", max_weight); + for(const auto& p : particles_){ + estimate_x += p.pose_.x() * p.weight(); + estimate_y += p.pose_.y() * p.weight(); + // printf("p %f\n", p.weight()); + // if(max_weight < p.weight()){ + // estimate_yaw = p.pose_.yaw(); + // max_weight = p.weight(); + // // printf("max %f\n", max_weight); + // } + estimate_yaw += p.pose_.yaw() * p.weight(); + } + estimated_pose_.set(estimate_x, estimate_y, estimate_yaw); + // printf("yaw %f\n", estimate_yaw); +} + +// 重みの正規化 +void Localizer::normalize_belief() +{ + const double weight_sum = calc_marginal_likelihood(); + for(auto& p : particles_){ + p.set_weight(p.weight()/weight_sum); + } +} + +// 膨張リセット（EMCLの場合） +void Localizer::expansion_resetting() +{ + +} + +// リサンプリング（系統サンプリング） +// 周辺尤度に応じてパーティクルをリサンプリング +void Localizer::resampling(const double alpha) +{ + // printf("resampling start\n"); + // パーティクルの重みを積み上げたリストを作成 + std::vector accum; + accum.push_back(particles_[0].weight()); + for(int i=1; i old(particles_); + int size = particles_.size(); + double step = accum.back() / size; + double start = (double)rand()/RAND_MAX * step; + // printf("set ok\n"); + + // particle数の動的変更 + // if(alpha>0.8 && particle_num_>max_particle_num_){ + // particle_num_ -= particle_num_/6; + // size = particle_num_; + // // printf("p_num -\n"); + // }else if(alpha<0.3 && particle_num_>min_particle_num_){ + // particle_num_ += particle_num_/3; + // size = particle_num_; + // // printf("p_num +\n"); + // } + + // サンプリングするパーティクルのインデックスを保持 + std::vector index; + index.reserve(particle_num_); + int num = 0; + for(int i=0; i publish(estimated_pose_msg_); + // printf("ep pub ok\n"); +} + +// パーティクルクラウドのパブリッシュ +// パーティクル数が変わる場合，リサイズする +void Localizer::publish_particles() +{ + is_visible_ = true; + if(is_visible_) + { + particle_cloud_msg_.poses.clear(); + geometry_msgs::msg::Pose pose; + + for(const auto& p : particles_) + { + pose.position.x = p.pose_.x(); + pose.position.y = p.pose_.y(); + + tf2::Quaternion q; + q.setRPY(0, 0, p.pose_.yaw()); + tf2::convert(q, pose.orientation); + + particle_cloud_msg_.poses.push_back(pose); + } + pub_particle_cloud_ -> publish(particle_cloud_msg_); + // printf("par pub ok\n"); + } +} \ No newline at end of file diff --git a/a_localizer/src/localizer_node.cpp b/a_localizer/src/localizer_node.cpp new file mode 100644 index 0000000..fda1245 --- /dev/null +++ b/a_localizer/src/localizer_node.cpp @@ -0,0 +1,17 @@ +#include "localizer/localizer.hpp" + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::Rate loop_rate(node->getOdomFreq()); + node->initialize(); + while(rclcpp::ok()) + { + node->process(); + rclcpp::spin_some(node); + loop_rate.sleep(); + } + rclcpp::shutdown(); + return 0; +} diff --git a/a_localizer/src/odom_model.cpp b/a_localizer/src/odom_model.cpp new file mode 100644 index 0000000..3f9dc7e --- /dev/null +++ b/a_localizer/src/odom_model.cpp @@ -0,0 +1,45 @@ +#include "localizer/odom_model.hpp" + +// デフォルトコンストラクタ +OdomModel::OdomModel() {} + +// コンストラクタ +OdomModel::OdomModel(const double ff, const double fr, const double rf, const double rr) + : engine_(seed_gen_()), std_norm_dist_(0.0, 1.0), fw_dev_(0.0), rot_dev_(0.0) +{ + fw_var_per_fw_ = pow(ff, 2.0); + fw_var_per_rot_ = pow(fr, 2.0); + rot_var_per_fw_ = pow(rf, 2.0); + rot_var_per_rot_ = pow(rr, 2.0); +} + +// 代入演算子 +OdomModel& OdomModel::operator =(const OdomModel& model) +{ + fw_var_per_fw_ = model.fw_var_per_fw_; + fw_var_per_rot_ = model.fw_var_per_rot_; + rot_var_per_fw_ = model.rot_var_per_fw_; + rot_var_per_rot_ = model.rot_var_per_rot_; + fw_dev_ = model.fw_dev_; + rot_dev_ = model.rot_dev_; + return *this; +} + +// 並進，回転に関する標準偏差の設定 +void OdomModel::set_dev(const double length, const double angle) +{ + fw_dev_ = sqrt(length*length*fw_var_per_fw_ + angle*angle*fw_var_per_rot_); + rot_dev_ = sqrt(length*length*rot_var_per_fw_ + angle*angle*rot_var_per_rot_); +} + +// 直進に関するノイズ（fw_dev_）の取得 +double OdomModel::get_fw_noise() +{ + return fw_dev_ * std_norm_dist_(engine_); +} + +// 回転に関するノイズ（rot_dev_）の取得 +double OdomModel::get_rot_noise() +{ + return rot_dev_ * std_norm_dist_(engine_); +} diff --git a/a_localizer/src/particle.cpp b/a_localizer/src/particle.cpp new file mode 100644 index 0000000..e46cba3 --- /dev/null +++ b/a_localizer/src/particle.cpp @@ -0,0 +1,120 @@ +#include "localizer/particle.hpp" + +// デフォルトコンストラクタ +Particle::Particle() : pose_(0.0, 0.0, 0.0) +{ + weight_ = 0.0; +} + +// コンストラクタ +Particle::Particle(const double x, const double y, const double yaw, const double weight) : pose_(x, y, yaw) +{ + weight_ = weight; +} + +// 代入演算子 +Particle& Particle::operator =(const Particle& p) +{ + pose_ = p.pose_; + weight_ = p.weight_; + return *this; +} + +// setter +void Particle::set_weight(const double weight) +{ + weight_ = weight; +} + +// 尤度関数 +// センサ情報からパーティクルの姿勢を尤度を算出 +double Particle::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& ignore_angle_range_list) +{ + double L = 0.0; // 尤度 + // センサ情報からパーティクルの姿勢を評価 + for(int i=0; i& ignore_angle_range_list) +{ + const int size = ignore_angle_range_list.size(); + + for(int i=0; i num:index - ignore_angle_range_list: [######, ######, ######] + ignore_angle_range_list: [#####, #####, #####] # フラグ flag_init_noise: true # 初期位置にノイズを加えるか diff --git a/localizer/package.xml b/localizer/package.xml index a5738a8..25ec7f8 100644 --- a/localizer/package.xml +++ b/localizer/package.xml @@ -1,8 +1,8 @@ - teama_localizer + team_a_localizer 0.0.0 - the teama_localizer package + the team_a_localizer package amsl TODO diff --git a/localizer/src/localizer.cpp b/localizer/src/localizer.cpp index 20de6b7..782e749 100644 --- a/localizer/src/localizer.cpp +++ b/localizer/src/localizer.cpp @@ -31,6 +31,7 @@ Localizer::Localizer() : Node("team_localizer") this->declare_parameter("fr", 0.0005); this->declare_parameter("rf", 0.13); this->declare_parameter("rr", 0.2); + this->declare_parameter("ignore_angle_range_list", std::vector({M_PI*0.22, M_PI*0.25, M_PI*0.72, M_PI*0.76})); // パラメータの取得 this->get_parameter("hz", hz_); @@ -55,6 +56,7 @@ Localizer::Localizer() : Node("team_localizer") this->get_parameter("fr", fr_); this->get_parameter("rf", rf_); this->get_parameter("rr", rr_); + this->get_parameter("ignore_angle_range_list", ignore_angle_range_list_); // Subscriberの設定 sub_map_ = this->create_subscription( @@ -89,6 +91,7 @@ void Localizer::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { map_ = *msg; flag_map_ = true; + printf("map call ok\n"); } // odometryのコールバック関数 @@ -97,6 +100,7 @@ void Localizer::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) prev_odom_ = last_odom_; last_odom_ = *msg; flag_odom_ = true; + // printf("%f -> %f\n", prev_odom_.pose.pose.position.x, last_odom_.pose.pose.position.x); } // laserのコールバック関数 @@ -104,6 +108,7 @@ void Localizer::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { laser_ = *msg; flag_laser_ = true; + // printf("laser call ok\n"); } // hz_を返す関数 @@ -132,6 +137,8 @@ void Localizer::initialize() // パーティクルの重みの初期化 reset_weight(); + + printf("particle weight %f\n", particles_[0].weight()); } // main文のループ内で実行される関数 @@ -159,7 +166,7 @@ double Localizer::normalize_angle(double angle) // ランダム変数生成関数（正規分布） double Localizer::norm_rv(const double mean, const double stddev) { - std::normal_distribution<> norm_dist(mean, stddev); + std::normal_distribution norm_dist(mean, stddev); return norm_dist(engine_); } @@ -167,7 +174,7 @@ double Localizer::norm_rv(const double mean, const double stddev) void Localizer::reset_weight() { for(auto& p : particles_){ - p.set_weight(1/particles_.size()); + p.set_weight(1.0/particles_.size()); } } @@ -175,6 +182,7 @@ void Localizer::reset_weight() // map座標系からみたbase_link座標系の位置と姿勢，odom座標系からみたbase_link座標系の位置と姿勢から計算 void Localizer::broadcast_odom_state() { + flag_broadcast_ = true; if(flag_broadcast_) { // TF Broadcasterの実体化 @@ -220,6 +228,8 @@ void Localizer::broadcast_odom_state() // tf情報をbroadcast(座標系の設定) odom_state_broadcaster->sendTransform(odom_state); + + // printf("broadcast ok\n"); } } @@ -245,15 +255,17 @@ void Localizer::motion_update() // 方向、移動量の計算 const double length = hypot(dx, dy); - const double direction = normalize_angle(atan2(dy, dx)); + const double direction = normalize_angle(atan2(dy, dx) - prev_yaw); + // printf("%f, %f\n", length, direction); // 標準偏差の設定 - odom_model_.set_dev(length, direction); + odom_model_.set_dev(length, dyaw); // パーティクルの位置更新 for(auto& p : particles_){ p.pose_.move(length, direction, dyaw, odom_model_.get_fw_noise(), odom_model_.get_rot_noise()); } + // printf("mu ok\n"); } // 観測更新 @@ -265,19 +277,31 @@ void Localizer::observation_update() p.set_weight(p.weight() * p.likelihood(map_, laser_, sensor_noise_ratio_, laser_step_, ignore_angle_range_list_)); } // パーティクル1つのレーザ1本における平均尤度を算出 - const double alpha = calc_marginal_likelihood()/laser_.ranges.size()/particles_.size(); - - estimate_pose(); - resampling(alpha_th_); + const double alpha = calc_marginal_likelihood()/((laser_.ranges.size()/laser_step_) * particles_.size()); + // printf("alpha %f\n", alpha); + + normalize_belief(); + if(alpha < alpha_th_ && reset_counter < reset_count_limit_){ + estimate_pose(); + expansion_resetting(); + reset_counter++; + } + else{ + estimate_pose(); + resampling(alpha); + reset_counter = 0; + } + // printf("ou ok\n"); } // 周辺尤度の算出 double Localizer::calc_marginal_likelihood() { double sum = 0.0; - for(auto& p : particles_){ + for(const auto& p : particles_){ sum += p.weight(); } + // printf("sum %f\n", sum); return sum; } @@ -285,15 +309,24 @@ double Localizer::calc_marginal_likelihood() // 算出方法は複数ある（平均，加重平均，中央値など...） void Localizer::estimate_pose() { - double estimate_x; - double estimate_y; - double estimate_yaw; - for(auto& p : particles_){ + double estimate_x = 0.0; + double estimate_y = 0.0; + double estimate_yaw = 0.0; + double max_weight = 0.0; + // printf("weight %f\n", max_weight); + for(const auto& p : particles_){ estimate_x += p.pose_.x() * p.weight(); estimate_y += p.pose_.y() * p.weight(); + // printf("p %f\n", p.weight()); + // if(max_weight < p.weight()){ + // estimate_yaw = p.pose_.yaw(); + // max_weight = p.weight(); + // // printf("max %f\n", max_weight); + // } estimate_yaw += p.pose_.yaw() * p.weight(); } estimated_pose_.set(estimate_x, estimate_y, estimate_yaw); + // printf("yaw %f\n", estimate_yaw); } // 重みの正規化 @@ -308,43 +341,74 @@ void Localizer::normalize_belief() // 膨張リセット（EMCLの場合） void Localizer::expansion_resetting() { - + for(auto& p : particles_){ + const double x = norm_rv(p.pose_.x(), expansion_x_dev_); + const double y = norm_rv(p.pose_.y(), expansion_y_dev_); + const double yaw = norm_rv(p.pose_.yaw(), expansion_yaw_dev_); + p.pose_.set(x, y, yaw); + p.pose_.normalize_angle(); + } + reset_weight(); } // リサンプリング（系統サンプリング） // 周辺尤度に応じてパーティクルをリサンプリング void Localizer::resampling(const double alpha) { + // printf("resampling start\n"); // パーティクルの重みを積み上げたリストを作成 std::vector accum; - for(int i=0; i old(particles_); int size = particles_.size(); - double step = accum.back() / size; - double start = rand()/RAND_MAX *step; + double step = sum_weight / size; + double start = (double)rand()/RAND_MAX * step; + // printf("set ok\n"); // particle数の動的変更 + // if(alpha>0.8 && particle_num_>max_particle_num_){ + // particle_num_ -= particle_num_/6; + // size = particle_num_; + // // printf("p_num -\n"); + // }else if(alpha<0.3 && particle_num_>min_particle_num_){ + // particle_num_ += particle_num_/3; + // size = particle_num_; + // // printf("p_num +\n"); + // } // サンプリングするパーティクルのインデックスを保持 std::vector index; - int num; - for(int i=0; i>particles_.size(); i++) + // index.reserve(particle_num_); + int num = 0; + int prev_num = 0; + for(int i=0; i publish(estimated_pose_msg_); + // printf("ep pub ok\n"); } // パーティクルクラウドのパブリッシュ // パーティクル数が変わる場合，リサイズする void Localizer::publish_particles() { + is_visible_ = true; if(is_visible_) { particle_cloud_msg_.poses.clear(); @@ -380,6 +446,7 @@ void Localizer::publish_particles() particle_cloud_msg_.poses.push_back(pose); } - pub_particle_cloud_ -> publish(particle_cloud_msg_); + pub_particle_cloud_ -> publish(particle_cloud_msg_); + // printf("par pub ok\n"); } } \ No newline at end of file diff --git a/localizer/src/odom_model.cpp b/localizer/src/odom_model.cpp index 6e99aeb..3f9dc7e 100644 --- a/localizer/src/odom_model.cpp +++ b/localizer/src/odom_model.cpp @@ -28,18 +28,18 @@ OdomModel& OdomModel::operator =(const OdomModel& model) // 並進，回転に関する標準偏差の設定 void OdomModel::set_dev(const double length, const double angle) { - fw_dev_ = sqrt(abs(length)*fw_var_per_fw_ + abs(angle)*fw_var_per_rot_); - rot_dev_ = sqrt(abs(length)*rot_var_per_fw_ + abs(angle)*rot_var_per_rot_); + fw_dev_ = sqrt(length*length*fw_var_per_fw_ + angle*angle*fw_var_per_rot_); + rot_dev_ = sqrt(length*length*rot_var_per_fw_ + angle*angle*rot_var_per_rot_); } // 直進に関するノイズ（fw_dev_）の取得 double OdomModel::get_fw_noise() { - return std_norm_dist_(engine_)*fw_dev_; + return fw_dev_ * std_norm_dist_(engine_); } // 回転に関するノイズ（rot_dev_）の取得 double OdomModel::get_rot_noise() { - return std_norm_dist_(engine_)*rot_dev_; + return rot_dev_ * std_norm_dist_(engine_); } diff --git a/localizer/src/particle.cpp b/localizer/src/particle.cpp index 95dad24..c8ae4ef 100644 --- a/localizer/src/particle.cpp +++ b/localizer/src/particle.cpp @@ -32,19 +32,25 @@ double Particle::likelihood(const nav_msgs::msg::OccupancyGrid& map, const senso const double sensor_noise_ratio, const int laser_step, const std::vector& ignore_angle_range_list) { double L = 0.0; // 尤度 + // double max = laser.range_max; // センサ情報からパーティクルの姿勢を評価 for(int i=0; i& ignore_angle_range_list) { const int size = ignore_angle_range_list.size(); + angle = abs(angle); for(int i=0; i