From 2a47625a282b1f72866a55b8a189223964bef590 Mon Sep 17 00:00:00 2001 From: Annika-wyt Date: Thu, 22 Feb 2024 20:33:59 +0100 Subject: [PATCH 1/4] update on wheel_encoder --- src/svea_core/src/svea/sensors.py | 7 ++++--- src/svea_sensors/scripts/wheel_encoder_reader.py | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/svea_core/src/svea/sensors.py b/src/svea_core/src/svea/sensors.py index 57173713..160b51a1 100644 --- a/src/svea_core/src/svea/sensors.py +++ b/src/svea_core/src/svea/sensors.py @@ -212,6 +212,7 @@ def _process_encoder_data(self, msg): # Linear velocity self.linear_velocity = (right_wheel_velocity + left_wheel_velocity)/2 self.linear_velocity *= direction + print("linear",self.linear_velocity) # Angular velocity angular_velocity = (right_wheel_velocity - left_wheel_velocity) angular_velocity /= self.axle_track @@ -225,16 +226,16 @@ def _process_direction(self, msg): direction_epsilon = self.tick_to_distance_coefficient * 0.5 # m/s if velocity > direction_epsilon: self.direction = 1 - elif velocity < direction_epsilon: + elif velocity < -(direction_epsilon): self.direction = -1 else: - self.directions = 0 + self.direction = 0 def _calc_wheel_velocity(self, ticks, time_delta): if time_delta == 0: return 0 distance = ticks * self.tick_to_distance_coefficient - velocity = (distance/time_delta) * 1e6 + velocity = (distance/(time_delta/1e3)) return velocity def add_callback(self, cb): diff --git a/src/svea_sensors/scripts/wheel_encoder_reader.py b/src/svea_sensors/scripts/wheel_encoder_reader.py index 230461d5..e36d6397 100755 --- a/src/svea_sensors/scripts/wheel_encoder_reader.py +++ b/src/svea_sensors/scripts/wheel_encoder_reader.py @@ -21,8 +21,8 @@ # Axle track in mm DEFAULT_AXLE_TRACK = 199.0 # Wheel radius in mm -DEFAULT_WHEEL_RADIUS = 60.0 -TICKS_PER_REVOLUTION = 60 +DEFAULT_WHEEL_RADIUS = 59.0 +TICKS_PER_REVOLUTION = 80 DEFAULT_LINEAR_COVARIANCE = 0.2 DEFAULT_ANGULAR_COVARIANCE = 0.4 From c5ed7c9791f023009b5357c1342a92dd81175540 Mon Sep 17 00:00:00 2001 From: Annika-wyt Date: Fri, 23 Feb 2024 16:56:55 +0100 Subject: [PATCH 2/4] updated sensors.py --- src/svea_core/src/svea/sensors.py | 38 ++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/src/svea_core/src/svea/sensors.py b/src/svea_core/src/svea/sensors.py index 160b51a1..43f36bf7 100644 --- a/src/svea_core/src/svea/sensors.py +++ b/src/svea_core/src/svea/sensors.py @@ -154,15 +154,21 @@ def __init__(self, self.axle_track = axle_track * mm_to_meter self.wheel_radius = wheel_radius self.ticks_per_revolution = ticks_per_revolution - self.tick_to_distance_coefficient = ( - wheel_radius * tau * mm_to_meter / ticks_per_revolution - ) + self.tick_to_distance_coefficient = (wheel_radius * tau * mm_to_meter / ticks_per_revolution) + self.direction_epsilon = self.tick_to_distance_coefficient * 0.5 # m/s # Storage fields self.direction = 1 self.linear_velocity = 0.0 self.angular_velocity = 0.0 self.r_wheel_velocity = 0.0 self.l_wheel_velocity = 0.0 + self.running_average_linear = 0.0 + self.running_average_angular = 0.0 + self.right_wheel_velocity = 0.0 + self.left_wheel_velocity = 0.0 + self.alpha = 2/(10+1) + self.encoder_time_scale = 1e-6 # in micros + self.threshold = 1e-5 # list of functions to call whenever a new reading comes in self.callbacks = [] @@ -210,23 +216,29 @@ def _process_encoder_data(self, msg): msg.left_time_delta) direction = self.direction # Linear velocity - self.linear_velocity = (right_wheel_velocity + left_wheel_velocity)/2 + self.right_wheel_velocity = self.alpha*right_wheel_velocity + (1-self.alpha)*self.right_wheel_velocity + self.left_wheel_velocity = self.alpha*left_wheel_velocity + (1-self.alpha)*self.left_wheel_velocity + self.linear_velocity = (self.right_wheel_velocity + self.left_wheel_velocity)/2 + self.running_average_linear = self.alpha*self.linear_velocity + (1-self.alpha)*self.running_average_linear self.linear_velocity *= direction - print("linear",self.linear_velocity) + if -self.threshold < self.running_average_linear < self.threshold: + self.linear_velocity = 0.0 # Angular velocity - angular_velocity = (right_wheel_velocity - left_wheel_velocity) - angular_velocity /= self.axle_track - angular_velocity *= direction - self.angular_velocity = angular_velocity + self.angular_velocity = (self.right_wheel_velocity - self.left_wheel_velocity)/self.axle_track + # print(self.right_wheel_velocity, self.left_wheel_velocity) + self.running_average_angular = self.alpha*self.angular_velocity + (1-self.alpha)*self.running_average_angular + if -self.threshold < self.angular_velocity < self.threshold: + self.angular_velocity = 0.0 + # print("linear",self.linear_velocity, "angular", self.angular_velocity, self.threshold) + for cb in self.callbacks: cb(self) def _process_direction(self, msg): velocity = msg.twist.twist.linear.x - direction_epsilon = self.tick_to_distance_coefficient * 0.5 # m/s - if velocity > direction_epsilon: + if velocity > self.direction_epsilon: self.direction = 1 - elif velocity < -(direction_epsilon): + elif velocity < -self.direction_epsilon: self.direction = -1 else: self.direction = 0 @@ -235,7 +247,7 @@ def _calc_wheel_velocity(self, ticks, time_delta): if time_delta == 0: return 0 distance = ticks * self.tick_to_distance_coefficient - velocity = (distance/(time_delta/1e3)) + velocity = (distance/(time_delta * self.encoder_time_scale)) return velocity def add_callback(self, cb): From b35317e70171b5907c0d0333dcf505955571cb94 Mon Sep 17 00:00:00 2001 From: Annika-wyt Date: Mon, 26 Feb 2024 12:08:11 +0100 Subject: [PATCH 3/4] minor updates on sensors.py --- src/svea_core/src/svea/sensors.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/svea_core/src/svea/sensors.py b/src/svea_core/src/svea/sensors.py index 43f36bf7..6f5e7715 100644 --- a/src/svea_core/src/svea/sensors.py +++ b/src/svea_core/src/svea/sensors.py @@ -215,21 +215,22 @@ def _process_encoder_data(self, msg): msg.left_ticks, msg.left_time_delta) direction = self.direction - # Linear velocity + self.right_wheel_velocity = self.alpha*right_wheel_velocity + (1-self.alpha)*self.right_wheel_velocity self.left_wheel_velocity = self.alpha*left_wheel_velocity + (1-self.alpha)*self.left_wheel_velocity + + # Linear velocity self.linear_velocity = (self.right_wheel_velocity + self.left_wheel_velocity)/2 self.running_average_linear = self.alpha*self.linear_velocity + (1-self.alpha)*self.running_average_linear self.linear_velocity *= direction if -self.threshold < self.running_average_linear < self.threshold: self.linear_velocity = 0.0 + # Angular velocity self.angular_velocity = (self.right_wheel_velocity - self.left_wheel_velocity)/self.axle_track - # print(self.right_wheel_velocity, self.left_wheel_velocity) self.running_average_angular = self.alpha*self.angular_velocity + (1-self.alpha)*self.running_average_angular if -self.threshold < self.angular_velocity < self.threshold: self.angular_velocity = 0.0 - # print("linear",self.linear_velocity, "angular", self.angular_velocity, self.threshold) for cb in self.callbacks: cb(self) From 6da3999cd364af1b5e030c24e7836e89a6bf5cd5 Mon Sep 17 00:00:00 2001 From: Annika-wyt Date: Mon, 26 Feb 2024 14:09:46 +0100 Subject: [PATCH 4/4] update param for global and local ekf --- src/svea_sensors/params/robot_localization/global_ekf.yaml | 2 +- src/svea_sensors/params/robot_localization/local_ekf.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/svea_sensors/params/robot_localization/global_ekf.yaml b/src/svea_sensors/params/robot_localization/global_ekf.yaml index ad412183..d496ebe0 100644 --- a/src/svea_sensors/params/robot_localization/global_ekf.yaml +++ b/src/svea_sensors/params/robot_localization/global_ekf.yaml @@ -91,7 +91,7 @@ twist1: /wheel_encoder_twist twist1_config: [false, false, false, false, false, false, true, false, false, - false, false, true, + false, false, false, false, false, false] twist1_nodelay: true twist1_differential: true diff --git a/src/svea_sensors/params/robot_localization/local_ekf.yaml b/src/svea_sensors/params/robot_localization/local_ekf.yaml index e43edf55..685633bd 100644 --- a/src/svea_sensors/params/robot_localization/local_ekf.yaml +++ b/src/svea_sensors/params/robot_localization/local_ekf.yaml @@ -92,7 +92,7 @@ twist1: /wheel_encoder_twist twist1_config: [false, false, false, false, false, false, true, false, false, - false, false, true, + false, false, false, false, false, false] twist1_nodelay: true twist1_differential: true