diff --git a/include/emcl/emcl.h b/include/emcl/emcl.h index 95a57cd..b65ebd5 100644 --- a/include/emcl/emcl.h +++ b/include/emcl/emcl.h @@ -44,13 +44,11 @@ struct EMCLParam float init_x = 0.0; float init_y = 0.0; float init_yaw = 0.0; - float init_x_dev = 0.0; - float init_y_dev = 0.0; - float init_yaw_dev = 0.0; + float init_position_dev = 0.0; + float init_orientation_dev = 0.0; float likelihood_th = 0.0; - float expansion_x_dev = 0.0; - float expansion_y_dev = 0.0; - float expansion_yaw_dev = 0.0; + float expansion_position_dev = 0.0; + float expansion_orientation_dev = 0.0; float sensor_noise_ratio = 0.0; }; @@ -72,8 +70,8 @@ struct OdomModelParam */ struct ScanParam { - float range_min; - float range_max; + float range_min = 0.0; + float range_max = 0.0; }; /** diff --git a/launch/test.yaml b/launch/test.yaml index 5beef6a..01a114e 100644 --- a/launch/test.yaml +++ b/launch/test.yaml @@ -1,7 +1,4 @@ # === EMCL === -# flag -flag_init_noise: true - # particle particle_num: 420 @@ -9,16 +6,14 @@ particle_num: 420 init_x: -2.0 init_y: -0.5 init_yaw: 0.0 -init_x_dev: 0.1 -init_y_dev: 0.1 -init_yaw_dev: 0.05 +init_position_dev: 0.1 +init_orientation_dev: 0.05 # resetting -likelihood_th: 0.0011 +likelihood_th: 0.002 reset_count_limit: 3 -expansion_x_dev: 0.07 -expansion_y_dev: 0.07 -expansion_yaw_dev: 0.15 +expansion_position_dev: 0.07 +expansion_orientation_dev: 0.15 # sensor laser_step: 4 diff --git a/src/emcl.cpp b/src/emcl.cpp index daaf6f8..913751a 100644 --- a/src/emcl.cpp +++ b/src/emcl.cpp @@ -117,8 +117,8 @@ void EMCL::initialize(const float init_x, const float init_y, const float init_y particles_.clear(); for (int i = 0; i < emcl_param_.particle_num; i++) particles_.push_back(Particle( - norm_dist(init_x, emcl_param_.init_x_dev), norm_dist(init_y, emcl_param_.init_y_dev), - norm_dist(init_yaw, emcl_param_.init_yaw_dev))); + norm_dist(init_x, emcl_param_.init_position_dev), norm_dist(init_y, emcl_param_.init_position_dev), + norm_dist(init_yaw, emcl_param_.init_orientation_dev))); reset_weight(); ROS_WARN("Initialized"); } @@ -277,8 +277,9 @@ void EMCL::expansion_resetting(void) { for (auto &p : particles_) p.pose_.set( - norm_dist(p.pose_.x(), emcl_param_.expansion_x_dev), norm_dist(p.pose_.y(), emcl_param_.expansion_y_dev), - norm_dist(p.pose_.yaw(), emcl_param_.expansion_yaw_dev)); + norm_dist(p.pose_.x(), emcl_param_.expansion_position_dev), + norm_dist(p.pose_.y(), emcl_param_.expansion_position_dev), + norm_dist(p.pose_.yaw(), emcl_param_.expansion_orientation_dev)); reset_weight(); } diff --git a/src/parameters.cpp b/src/parameters.cpp index a1cbf56..cad8389 100644 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -13,25 +13,23 @@ void EMCL::load_params(void) { // EMCL // - e - - private_nh_.param("expansion_x_dev", emcl_param_.expansion_x_dev, 0.05); - private_nh_.param("expansion_y_dev", emcl_param_.expansion_y_dev, 0.05); - private_nh_.param("expansion_yaw_dev", emcl_param_.expansion_yaw_dev, 0.01); + private_nh_.param("expansion_position_dev", emcl_param_.expansion_position_dev, 0.07); + private_nh_.param("expansion_orientation_dev", emcl_param_.expansion_orientation_dev, 0.15); // - i - private_nh_.param("init_x", emcl_param_.init_x, 0.0); - private_nh_.param("init_x_dev", emcl_param_.init_x_dev, 0.5); private_nh_.param("init_y", emcl_param_.init_y, 0.0); - private_nh_.param("init_y_dev", emcl_param_.init_y_dev, 0.65); private_nh_.param("init_yaw", emcl_param_.init_yaw, 0.0); - private_nh_.param("init_yaw_dev", emcl_param_.init_yaw_dev, 0.5); + private_nh_.param("init_position_dev", emcl_param_.init_position_dev, 0.1); + private_nh_.param("init_orientation_dev", emcl_param_.init_orientation_dev, 0.05); // - l - - private_nh_.param("laser_step", emcl_param_.laser_step, 10); - private_nh_.param("likelihood_th", emcl_param_.likelihood_th, 0.02); + private_nh_.param("laser_step", emcl_param_.laser_step, 4); + private_nh_.param("likelihood_th", emcl_param_.likelihood_th, 0.002); // - p - private_nh_.param("particle_num", emcl_param_.particle_num, 420); // - r - - private_nh_.param("reset_count_limit", emcl_param_.reset_count_limit, 5); + private_nh_.param("reset_count_limit", emcl_param_.reset_count_limit, 3); // - s - - private_nh_.param("sensor_noise_ratio", emcl_param_.sensor_noise_ratio, 0.03); + private_nh_.param("sensor_noise_ratio", emcl_param_.sensor_noise_ratio, 0.02); // - u - private_nh_.param("use_cloud", emcl_param_.use_cloud, false); @@ -52,15 +50,13 @@ void EMCL::load_params(void) void EMCL::print_params(void) { ROS_INFO("EMCL Parameters:"); - ROS_INFO_STREAM(" expansion_x_dev: " << emcl_param_.expansion_x_dev); - ROS_INFO_STREAM(" expansion_y_dev: " << emcl_param_.expansion_y_dev); - ROS_INFO_STREAM(" expansion_yaw_dev: " << emcl_param_.expansion_yaw_dev); + ROS_INFO_STREAM(" expansion_position_dev: " << emcl_param_.expansion_position_dev); + ROS_INFO_STREAM(" expansion_orientation_dev: " << emcl_param_.expansion_orientation_dev); ROS_INFO_STREAM(" init_x: " << emcl_param_.init_x); - ROS_INFO_STREAM(" init_x_dev: " << emcl_param_.init_x_dev); ROS_INFO_STREAM(" init_y: " << emcl_param_.init_y); - ROS_INFO_STREAM(" init_y_dev: " << emcl_param_.init_y_dev); ROS_INFO_STREAM(" init_yaw: " << emcl_param_.init_yaw); - ROS_INFO_STREAM(" init_yaw_dev: " << emcl_param_.init_yaw_dev); + ROS_INFO_STREAM(" init_position_dev: " << emcl_param_.init_position_dev); + ROS_INFO_STREAM(" init_orientation_dev: " << emcl_param_.init_orientation_dev); ROS_INFO_STREAM(" laser_step: " << emcl_param_.laser_step); ROS_INFO_STREAM(" likelihood_th: " << emcl_param_.likelihood_th); ROS_INFO_STREAM(" particle_num: " << emcl_param_.particle_num);