From 8796ec98ddd5fbe59a58cadded7aa8bd8dd9623e Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Wed, 5 May 2021 12:57:13 -0600 Subject: [PATCH 1/2] Updated Wheel encoders for Round Measured the wheel radius and wheel base and updated the wheel encoder node to match. Noetic displays a timestamp warning for every message that is not later than the last message recieved. This makes the repl unusable. Added a minimum delta time between publishing so avoid that problem. Still need to figure out the minimum delta. This issue of spamming warnings is under active discussion on the devs github. --- src/localisation/src/wheel_encoder.py | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index e06daf3..a593eea 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -17,6 +17,10 @@ class WheelEncoder: def __init__(self): self.name = rospy.get_param('rover_name', default='small_scout_1') + print("Subscribing to /{}/imu".format(self.name)) + print("Subscribing to /{}/joint_states".format(self.name)) + print("Publishing to /{}/odom".format(self.name)) + rospy.Subscriber("/{}/imu".format(self.name), Imu, self.imuCallback) rospy.Subscriber("/{}/joint_states".format(self.name), JointState, self.jointStatesCallback) self.odom_pub = rospy.Publisher("/{}/odom".format(self.name), Odometry, queue_size=50) @@ -29,8 +33,10 @@ def __init__(self): self.previous_front_right_wheel_angle = 0 self.previous_back_left_wheel_angle = 0 self.previous_back_right_wheel_angle = 0 - self.wheel_radius = 0.27 # meters - self.track_width = 1.75 # meters + #self.wheel_radius = 0.27 # meters + self.wheel_radius = 0.17 # meters + # self.track_width = 1.75 # meters + self.track_width = 1 # meters self.sample_rate = 1 self.message_count = 0 @@ -59,7 +65,11 @@ def jointStatesCallback(self, data): current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() - + + # Check that sufficient time has passed to avoid duplicate time stamps + if dt < 1: # Requre at least 1 second to have passed between function execution + return + ### Calculate angular velocity ### # Angular displacement in radians - could use all four wheels, start with just two @@ -117,12 +127,15 @@ def jointStatesCallback(self, data): #self.theta += v_wtheta # Composing your odometry message - + # The odometry message contains pose and twist information. The pose represents your current robot pose in the the odom frame (x, y, theta). # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.theta) + # Check if the timestamp will be different + + # first, we'll publish the transform over tf self.odom_broadcaster.sendTransform( (self.x, self.y, 0.), @@ -172,6 +185,8 @@ def shutdownHandler(): rospy.init_node('wheel_encoder', anonymous=True) + print("Wheel encoder node started") + # Register shutdown handler (includes ctrl-c handling) rospy.on_shutdown( shutdownHandler ) From 17a3a2cb6ffae23c17c608db78f2d803bfb3cdd8 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Fri, 21 May 2021 16:43:15 -0600 Subject: [PATCH 2/2] Handle Wheel pose in degrees and IMU offset For some reason the pose handler for the wheels is in degrees so had to add a conversion to radians (this must be new) The IMU's yaw is offset from the rover by 45 degrees Had to add a factor of 2pi. I can see no reason why this was needed but results in a usable wheel encoder. --- src/localisation/src/wheel_encoder.py | 54 ++++++++++++++++++--------- 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index a593eea..b71e854 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -2,6 +2,7 @@ # Generates odometry from wheel poses +import math import sys import tf.transformations as transform from sensor_msgs.msg import JointState, Imu @@ -38,6 +39,7 @@ def __init__(self): # self.track_width = 1.75 # meters self.track_width = 1 # meters self.sample_rate = 1 + self.msg_min_delta = 0.1 # seconds <--- Changing this from 0.1 spoils the accuracy. That should not be the case! self.message_count = 0 def imuCallback(self, data): @@ -47,7 +49,7 @@ def imuCallback(self, data): data.orientation.z, data.orientation.w) roll, pitch, yaw = transform.euler_from_quaternion(q) # in [-pi, pi] - self.theta = yaw + self.theta = yaw + 0.78 # rotate 45 degrees def jointStatesCallback(self, data): @@ -57,17 +59,18 @@ def jointStatesCallback(self, data): if self.message_count % self.sample_rate != 0: return - - back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ] - back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ] - front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ] - front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ] + + # Divide by 180 to get radians + back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ]*math.pi/180 + back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ]*math.pi/180 + front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ]*math.pi/180 + front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ]*math.pi/180 current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() # Check that sufficient time has passed to avoid duplicate time stamps - if dt < 1: # Requre at least 1 second to have passed between function execution + if dt < self.msg_min_delta: # Specify the minimum time between messages for function execution (to prevent timestamp warning killing the repl) return ### Calculate angular velocity ### @@ -102,10 +105,16 @@ def jointStatesCallback(self, data): v_left = omega_left * self.wheel_radius v_right = omega_right * self.wheel_radius - v_rx = ( v_right + v_left ) /2 + print("Wheel Radius: ", self.wheel_radius) + + v_rx = ( v_right + v_left ) / 2.0 v_ry = 0 + + print("Velocity: ", v_rx, " m/s") + print("Yaw: ", self.theta) + # We increase the track width by a factor of 4 to match empirical tests - v_rtheta = ( v_right - v_left ) / self.track_width + v_rtheta = ( v_right - v_left ) / 2*self.track_width # Velocities and pose in the odom frame # The velocities expressed in the robot base frame can be transformed into the odom frame. @@ -113,15 +122,26 @@ def jointStatesCallback(self, data): # We are operating in the x-y plane, we ignore altitude - which will result in errors as the robot climbs up and down. # Integrate the velocities over time (multiply by the time differential dt and sum) - v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt - v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt - #v_wx = v_rx*cos(self.theta+v_rtheta/2) - #v_wy = v_rx*sin(self.theta+v_rtheta/2) + #v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt + #v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt - v_wtheta = v_rtheta * dt + # We know our heading from the imu - no need to calculate it from wheel velocities + v_wx = v_rx*cos(self.theta) + v_wy = v_rx*sin(self.theta) + + print("X displacement: ", v_wx, " m") + print("Y displacement: ", v_wy, " m") + print("Delta_t: ", dt, " s") + + print("Back Left Wheel Angle: ", back_left_wheel_angle) + + #v_wtheta = v_rtheta * dt - self.x += v_wx - self.y += v_wy + self.x += v_wx*2*math.pi # I don't understand why this 2pi factor is needed + self.y += v_wy*2*math.pi + + print("X coord: ", self.x) + print("Y coord: ", self.y) # Replaced with IMU theta #self.theta += v_wtheta @@ -176,7 +196,7 @@ def jointStatesCallback(self, data): self.previous_front_left_wheel_angle = front_left_wheel_angle self.previous_front_right_wheel_angle = front_right_wheel_angle self.previous_back_left_wheel_angle = back_left_wheel_angle - self.previous_back_right_wheel_angle = back_right_wheel_angle + self.previous_back_right_wheel_angle = back_right_wheel_angle def shutdownHandler(): print("Wheel encoder shutting down.")