Update param

ToshikiNakamura0412 · May 26, 2024 · be6a33c · be6a33c
1 parent 376d6ee
commit be6a33c
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 38 deletions.
diff --git 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
@@ -1,24 +1,19 @@
 # === EMCL ===
-# flag
-flag_init_noise: true
-
 # particle
 particle_num: 420
 
 # initial pose
 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
@@ -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
@@ -13,25 +13,23 @@ void EMCL::load_params(void)
 {
   // EMCL
   // - e -
-  private_nh_.param<float>("expansion_x_dev", emcl_param_.expansion_x_dev, 0.05);
-  private_nh_.param<float>("expansion_y_dev", emcl_param_.expansion_y_dev, 0.05);
-  private_nh_.param<float>("expansion_yaw_dev", emcl_param_.expansion_yaw_dev, 0.01);
+  private_nh_.param<float>("expansion_position_dev", emcl_param_.expansion_position_dev, 0.07);
+  private_nh_.param<float>("expansion_orientation_dev", emcl_param_.expansion_orientation_dev, 0.15);
   // - i -
   private_nh_.param<float>("init_x", emcl_param_.init_x, 0.0);
-  private_nh_.param<float>("init_x_dev", emcl_param_.init_x_dev, 0.5);
   private_nh_.param<float>("init_y", emcl_param_.init_y, 0.0);
-  private_nh_.param<float>("init_y_dev", emcl_param_.init_y_dev, 0.65);
   private_nh_.param<float>("init_yaw", emcl_param_.init_yaw, 0.0);
-  private_nh_.param<float>("init_yaw_dev", emcl_param_.init_yaw_dev, 0.5);
+  private_nh_.param<float>("init_position_dev", emcl_param_.init_position_dev, 0.1);
+  private_nh_.param<float>("init_orientation_dev", emcl_param_.init_orientation_dev, 0.05);
   // - l -
-  private_nh_.param<int>("laser_step", emcl_param_.laser_step, 10);
-  private_nh_.param<float>("likelihood_th", emcl_param_.likelihood_th, 0.02);
+  private_nh_.param<int>("laser_step", emcl_param_.laser_step, 4);
+  private_nh_.param<float>("likelihood_th", emcl_param_.likelihood_th, 0.002);
   // - p -
   private_nh_.param<int>("particle_num", emcl_param_.particle_num, 420);
   // - r -
-  private_nh_.param<int>("reset_count_limit", emcl_param_.reset_count_limit, 5);
+  private_nh_.param<int>("reset_count_limit", emcl_param_.reset_count_limit, 3);
   // - s -
-  private_nh_.param<float>("sensor_noise_ratio", emcl_param_.sensor_noise_ratio, 0.03);
+  private_nh_.param<float>("sensor_noise_ratio", emcl_param_.sensor_noise_ratio, 0.02);
   // - u -
   private_nh_.param<bool>("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);