def lines_callback(self, msg):
"""Callback to a LaserScan message with topic "scan"."""
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points) / 2):
# Get start and end points
pt1 = msg.points[2 * i]
pt2 = msg.points[2 * i + 1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines, self.pub_lines,
frame='/robot',
time=msg.header.stamp,
ns='lines_robot', color=(0, 0, 1))
def lines_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points)/2):
# Get start and end points
pt1 = msg.points[2*i]
pt2 = msg.points[2*i+1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines, self.pub_lines,
frame = '/robot',
time = msg.header.stamp,
ns = 'lines_robot', color = (0,0,1))
def cbk_lines(self, msg):
"""Republish the laser scam in the /robot frame."""
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points)/2):
# Get start and end points
pt1 = msg.points[2*i]
pt2 = msg.points[2*i+1]
# Transform to robot frame
pt1R = funcs.comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = funcs.comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
funcs.publish_lines(self.lines, self.pub_lines, frame='robot',
time=msg.header.stamp, ns='lines_robot',
color=(0, 0, 1))
def predict(self, uk):
"""
Implement the prediction equations of the EKF.
Saves the new robot state and uncertainty.
Input:
uk : numpy array [shape (3,) with (dx, dy, dtheta)]
"""
# TODO: Program this function
################################################################
# Check website for numpy help:
# http://wiki.scipy.org/NumPy_for_Matlab_Users
# 1. Update self.xk and self.Pk using uk and self.Qk
# can use comp() from funtions.py
self.xk = funcs.comp(self.xk, uk)
W = np.array([[np.cos(self.xk[2]), -np.sin(self.xk[2]), 0],
[np.sin(self.xk[2]),
np.cos(self.xk[2]), 0], [0, 0, 1]])
A = np.array([[
1, 0, -np.dot(np.sin(self.xk[2]), uk[0]) -
np.dot(np.cos(self.xk[2]), uk[1])
],
[
0, 1,
np.dot(np.cos(self.xk[2]), uk[0]) -
np.dot(np.sin(self.xk[2]), uk[1])
], [0, 0, 1]])
self.Pk = np.dot(np.dot(A, self.Pk), A.T) + np.dot(
np.dot(W, self.Qk), W.T)
def predict(self, uk):
'''
Predicts the position of the robot according to the previous position and the odometry measurements. It also updates the uncertainty of the position
'''
#TODO: Program this function
# - Update self.xk and self.Pk using uk and self.Qk
# Compound robot with odometry
a = comp(self.xk[0:3], uk)
self.xk = np.append(a, self.xk[3:])
# Compute jacobians of the composition with respect to robot (A_k)
# and odometry (W_k)
W = np.array([[np.cos(self.xk[2]), -np.sin(self.xk[2]), 0],
[np.sin(self.xk[2]),
np.cos(self.xk[2]), 0], [0, 0, 1]])
A = np.array([[
1, 0, -np.dot(np.sin(self.xk[2]), uk[0]) -
np.dot(np.cos(self.xk[2]), uk[1])
],
[
0, 1,
np.dot(np.cos(self.xk[2]), uk[0]) -
np.dot(np.sin(self.xk[2]), uk[1])
], [0, 0, 1]])
n = self.get_number_of_features_in_map()
# Prepare the F_k and G_k matrix for the new uncertainty computation
F_k = np.eye(3 + 2 * n)
G_k = np.zeros((3 + 2 * n, 3))
F_k[0:3, 0:3] = A
G_k[0:3, 0:3] = W
# Compute uncertainty
# Update the class variables
self.Pk = np.dot(np.dot(F_k, self.Pk), F_k.T) + np.dot(
np.dot(G_k, self.Qk), G_k.T)
def predict(self, uk):
'''
Predicts the position of the robot according to the previous position and the odometry measurements. It also updates the uncertainty of the position
'''
#TODO: Program this function
# - Update self.xk and self.Pk using uk and self.Qk
# Compound robot with odometry
xk = comp(self.xk[0:3], uk)
# Compute jacobians of the composition with respect to robot (A_k)
# and odometry (W_k)
Ak = np.array(
[[1, 0, -sin(self.xk[2]) * uk[0] - cos(self.xk[2]) * uk[1]],
[0, 1, cos(self.xk[2]) * uk[0] - sin(self.xk[2]) * uk[1]],
[0, 0, 1]])
Wk = np.array([[cos(self.xk[2]), -sin(self.xk[2]), 0],
[sin(self.xk[2]), cos(self.xk[2]), 0], [0, 0, 1]])
# Prepare the F_k and G_k matrix for the new uncertainty computation
num_fea = self.get_number_of_features_in_map()
Fk = np.eye(3 + 2 * num_fea)
Fk[0:3, 0:3] = Ak
Gk = np.zeros((3 + 2 * num_fea, 3))
Gk[0:3, 0:3] = Wk
# Compute uncertainty
Pk = np.dot(np.dot(Fk, self.Pk), np.transpose(Fk)) + np.dot(
np.dot(Gk, self.Qk), np.transpose(Gk))
# Update the class variables
self.xk[0:3] = xk
self.Pk = Pk
def predict(self, uk):
'''
Predicts the position of the robot according to the previous position and the odometry measurements. It also updates the uncertainty of the position
'''
#TODO: Program this function
# - Update self.xk and self.Pk using uk and self.Qk
# Compound robot with odometry
a=funcs.comp(self.xk[0:3],uk)
self.xk = np.append(a ,self.xk[3:])
# Compute jacobians of the composition with respect to robot (A_k)
# and odometry (W_k)
c1=-np.sin(self.xk[2])*uk[0]-np.cos(self.xk[2])*uk[1]
c2=np.cos(self.xk[2])*uk[0]-np.sin(self.xk[2])*uk[1]
Ak=np.array([ [1, 0, c1],[0, 1, c2],[0 ,0, 1]])
a1=np.cos(self.xk[2])
a2=np.sin(self.xk[2])
Wk=np.array([ [a1, -a2, 0],[a2, a1, 0],[0 ,0, 1]])
# Prepare the F_k and G_k matrix for the new uncertainty computation
n=self.get_number_of_features_in_map()
F_k=np.eye(3+2*n)
G_k=np.zeros((3+2*n,3))
F_k[0:3,0:3]=Ak
G_k[0:3,0:3]=Wk
# Compute uncertainty
self.Pk=np.dot(np.dot(F_k,self.Pk),F_k.T)+np.dot(np.dot(G_k,self.Qk),G_k.T)
def predict(self, uk):
"""
Implement the prediction equations of the EKF.
Saves the new robot state and uncertainty.
Input:
uk : numpy array [shape (3,) with (dx, dy, dtheta)]
"""
# TODO: Program this function
################################################################
# Robot state
xk = funcs.comp(self.xk, uk)
# Uncertainty
Ak = np.array(
[[1, 0, -sin(self.xk[2]) * uk[0] - cos(self.xk[2]) * uk[1]],
[0, 1, cos(self.xk[2]) * uk[0] - sin(self.xk[2]) * uk[1]],
[0, 0, 1]])
Wk = np.array([[cos(self.xk[2]), -sin(self.xk[2]), 0],
[sin(self.xk[2]), cos(self.xk[2]), 0], [0, 0, 1]])
#self.Pk = Ak * self.Pk * np.transpose(Ak) + Wk * self.Qk * np.transpose(Wk)
self.Pk = np.dot(np.dot(Ak, self.Pk), np.transpose(Ak)) + np.dot(
np.dot(Wk, self.Qk), np.transpose(Wk))
self.xk = xk
def predict(self, uk):
"""
Implement the prediction equations of the EKF.
Saves the new robot state and uncertainty.
Input:
uk : numpy array [shape (3,) with (dx, dy, dtheta)]
"""
################################################################
# Check website for numpy help:
# http://wiki.scipy.org/NumPy_for_Matlab_Users
# 1. Update self.xk and self.Pk using uk and self.Qk
# can use comp() from funtions.py
# Predict the mean of state vector
self.xk = funcs.comp(self.xk,uk)
Ak = np.array([[1,0, - math.sin(self.xk[2]) * uk[0] - math.cos(self.xk[2]) * uk[1]],[0,1,math.cos(self.xk[2]) * uk[0] - math.sin(self.xk[2]) * uk[1]],[0,0,1]])
Wk = np.array([[math.cos(self.xk[2]), - math.sin(self.xk[2]),0],[math.sin(self.xk[2]),math.cos(self.xk[2]),0],[0,0,1]])
P1 = np.dot(Ak,self.Pk)
P1 = np.dot(P1,np.transpose(Ak))
P2 = np.dot(Wk,self.Qk)
P2 = np.dot(P2,np.transpose(Wk))
# Predict the uncertainty
self.Pk = P1 + P2