Adding noise levels

common/hal_interfaces/hal_interfaces/general/noise_odometry.py

@@ -3,7 +3,6 @@
 from math import asin, atan2, pi
 import nav_msgs.msg
 
-
 ### AUXILIARY FUNCTIONS ###
 class Pose3d:
 
@@ -127,12 +126,12 @@ def odometry2Pose3D(odom):
 
     return pose
 
-def gaussian_noise(x,mu=0.0,std=0.1):
-    noise = np.random.normal(mu, std) * 0.1
+def gaussian_noise(x,mu=0.0,std=0.1, noise_level = 0.01):
+    noise = np.random.normal(mu, std) * 0.01
     x_noisy = x + noise
     return x_noisy 
 
-def add_noise(last_pose, new_pose, base_odom):
+def add_noise(last_pose, new_pose, base_odom, noise_level):
 
     # First odom is real
     if (last_pose == None):
@@ -151,9 +150,9 @@ def add_noise(last_pose, new_pose, base_odom):
     mov_roll = quat2Roll(new_ori.w, new_ori.x, new_ori.y, new_ori.z) - quat2Roll(old_ori.w, old_ori.x, old_ori.y, old_ori.z)
 
     # Add noise
-    mov_x = gaussian_noise(mov_x)
-    mov_y = gaussian_noise(mov_y)
-    mov_yaw = gaussian_noise(mov_yaw)
+    mov_x = gaussian_noise(mov_x, noise_level = noise_level)
+    mov_y = gaussian_noise(mov_y, noise_level = noise_level)
+    mov_yaw = gaussian_noise(mov_yaw, noise_level = noise_level)
 
     # Get new odom angle
     ori = base_odom.pose.pose.orientation
@@ -189,11 +188,14 @@ def __init__(self, topic):
         self.last_pose_ = None
         self.noisy_pose = nav_msgs.msg.Odometry()
 
+        ### Control the amount of noise ###
+        self.noise_level = 0.01 # 0.1 = a lot
+
     def listener_callback(self, msg):
         # First odom is real
         # Second odom is movement from first real to second real + noise + first odom
         # Third odom is movement from second real to third real + noise + second odom
-        self.noisy_pose = add_noise(self.last_pose_, msg, self.noisy_pose)
+        self.noisy_pose = add_noise(self.last_pose_, msg, self.noisy_pose, self.noise_level)
         self.last_pose_ = msg
 
     def getPose3d(self):