Skip to content

Updated Wheel encoders for Round #203

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
gmfricke wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Updated Wheel encoders for Round #203

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8796ec9
Updated Wheel encoders for Round
gmfricke May 5, 2021
17a3a2c
Handle Wheel pose in degrees and IMU offset
gmfricke May 21, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
75 changes: 55 additions & 20 deletions src/localisation/src/wheel_encoder.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -17,6 +18,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)
Expand All @@ -29,9 +34,12 @@ 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.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):
Expand All @@ -41,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):
Expand All @@ -51,15 +59,20 @@ 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 < self.msg_min_delta: # Specify the minimum time between messages for function execution (to prevent timestamp warning killing the repl)
return

### Calculate angular velocity ###

# Angular displacement in radians - could use all four wheels, start with just two
Expand Down Expand Up @@ -92,37 +105,57 @@ 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.
# To transform velocities into the odom frame we only need to rotate them according to the current robot orientation.
# 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

# 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.),
Expand Down Expand Up @@ -163,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.")
Expand All @@ -172,6 +205,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 )

Expand Down