measurement noise as launch arg

Author: MatCat960

launch/distri_gmm_calculator.launch.py

@@ -27,8 +27,9 @@ def generate_launch_description():
     TARGETS_NUM = 4
     SENS_RANGE = 5.0
     COMM_RANGE = 7.0
+    NOISE_DEV = 0.5
     ENV_SIZE = 20.0
-    GRAPHICS_ON = True
+    GRAPHICS_ON = False
 
     GUI = False
 
@@ -44,6 +45,7 @@ def generate_launch_description():
                         {"TARGETS_NUM": TARGETS_NUM},
                         {"SENS_RANGE": SENS_RANGE},
                         {"COMM_RANGE": COMM_RANGE},
+                        {"NOISE_DEV": NOISE_DEV},
                         {"ENV_SIZE": ENV_SIZE},
                         {"GRAPHICS_ON": GRAPHICS_ON}],
             output='screen')

launch/distributed_gmm.launch.py

@@ -19,7 +19,7 @@
 def generate_launch_description():
     TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
     ROBOTS_NUM = 8
-    ROBOT_RANGE = 7.0
+    ROBOT_RANGE = 10.0
     AREA_SIZE_x = 20.0
     AREA_SIZE_y = 20.0
     AREA_LEFT = -10.0
@@ -32,6 +32,7 @@ def generate_launch_description():
         n = Node(
             package='gmm_coverage',
             node_executable='distributed_gmm',
+            name='distributed_gmm_'+str(i),
             remappings=[('/gaussian_mixture_model', '/gaussian_mixture_model_'+str(i))],
             parameters=[{"ROBOTS_NUM": ROBOTS_NUM}, {"ROBOT_RANGE": ROBOT_RANGE}, {"AREA_SIZE_x": AREA_SIZE_x}, {"AREA_SIZE_y": AREA_SIZE_y}, {"AREA_LEFT": AREA_LEFT}, {"AREA_BOTTOM": AREA_BOTTOM}, {"SIM": SIM}, {"ID": i}, {"GUI": GUI}],
             output='screen')

src/distributed_gmm.cpp

@@ -62,7 +62,7 @@ const double b = 0.025;                 //for differential drive control (only i
 //------------------------------------------------------------------------
 const float CONVERGENCE_TOLERANCE = 0.1;
 //------------------------------------------------------------------------
-const int shutdown_timer = 10;           //count how many seconds to let the robots stopped before shutting down the node
+const int shutdown_timer = 30;           //count how many seconds to let the robots stopped before shutting down the node
 
 
 bool IsPathExist(const std::string &s)

src/gmm_calc.cpp

@@ -64,6 +64,8 @@ class Supervisor : public rclcpp::Node
         this->get_parameter("SENS_RANGE", SENS_RANGE);
         this->declare_parameter<double>("COMM_RANGE", 5.0);
         this->get_parameter("COMM_RANGE", COMM_RANGE);
+        this->declare_parameter<double>("NOISE_STD", 1.0);
+        this->get_parameter("NOISE_STD", NOISE_STD);
         this->declare_parameter<double>("ENV_SIZE", 20.0);
         this->get_parameter("ENV_SIZE", ENV_SIZE);
         this->declare_parameter<bool>("GRAPHICS_ON", true);
@@ -157,6 +159,7 @@ class Supervisor : public rclcpp::Node
     int TARGETS_NUM;
     double COMM_RANGE;
     double SENS_RANGE;
+    double NOISE_STD;
     double ENV_SIZE;
     bool GRAPHICS_ON;
     GaussianMixtureModel gmm;
@@ -238,9 +241,8 @@ Eigen::VectorXd Supervisor::addNoise(Eigen::VectorXd point)
     // Generate random inputs
     std::random_device rd;
     std::mt19937 gen(rd());
-    double dev = 1.0;
-    std::normal_distribution<double> dist_x(point(0), dev);
-    std::normal_distribution<double> dist_y(point(1), dev);
+    std::normal_distribution<double> dist_x(point(0), NOISE_STD);
+    std::normal_distribution<double> dist_y(point(1), NOISE_STD);
 
     Eigen::VectorXd noisy_point(2);
     noisy_point << dist_x(gen), dist_y(gen);