def odometry_callback(self, data):
global update_state_lock
with self.update_state_lock:
ticks = data.ticks
tps = data.tps
angle = (math.pi * data.angle) / 180.
if config.ROS_STATE['DEBUG']:
rospy.loginfo("received odometry: " + str(ticks) + " " + str(tps) + " " + str(angle))
# easy update
if self.ticks == None: # protect first update
self.ticks = ticks
ticks_moved = ticks - self.ticks
self.ticks = ticks
meters_moved = float(ticks_moved) / config.CAR['TICKS_PER_METER']
x_moved = meters_moved * math.cos(self.state.theta)
y_moved = meters_moved * math.sin(self.state.theta)
theta_moved = (meters_moved / config.CAR['L']) * math.tan(angle)
if config.ROS_STATE['DEBUG']:
rospy.loginfo("moved odometry (meter): " + str(x_moved) + " " + str(y_moved) + " " + str(theta_moved))
(dlat, dlon) = car_helper.meter2gps(x_moved, y_moved)
self.state.x = self.state.x + dlon
self.state.y = self.state.y + dlat
self.state.theta = (self.state.theta + theta_moved) % (2*math.pi)
pub.publish(self.state)
def predictionUpdate(self, meters_moved, steering_angle):
sintheta = math.sin(self.x.item(2))
costheta = math.cos(self.x.item(2))
# perform prediction update
x_moved = meters_moved * costheta
y_moved = meters_moved * sintheta
theta_moved = (meters_moved / config.CAR['L']) * math.tan(steering_angle)
# TODO: to speed up the program use fixed ratio, set at start program
(dlat, dlon) = car_helper.meter2gps(x_moved, y_moved, self.x.item(0), self.x.item(1))
#(dlat, dlon) = (x_moved, y_moved) debug meters
predicted_movement = np.array([
[dlat],
[dlon],
[theta_moved]
])
self.x = self.x + predicted_movement
# TODO: normalize angle
if self.x.item(2) > TWO_PI:
self.x[2] = self.x.item(2) - TWO_PI
# TODO: how to implement rotation? (1 is near 359 degrees)
# calculate Jacobian
F = np.array([
[1.,0.,-1.*sintheta],
[0.,1.,costheta],
[0.,0.,1.]
])
# update covariance
self.sigma = np.dot(F, np.dot(self.sigma, F.T)) + self.Q
