def tfPolarLine(self, tf, line):
'''
Transforms a polar line in the robot frame to a polar line in the
world frame
'''
# Decompose transformation
# Decompose transformation
x_x = tf[0]
x_y = tf[1]
x_ang = tf[2]
# Compute the new phi
phi = angle_wrap(line[1] + x_ang)
rho_ = line[0] + x_x * np.cos(phi) + x_y * np.sin(phi)
sign = 1
if rho_ < 0:
rho_ = -rho_
phi = angle_wrap(phi + pi)
sign = -1
# Allocate jacobians
H_tf = np.zeros((2, 3))
H_line = np.eye(2)
# TODO: Evaluate jacobian respect to transformation
H_tf = np.array(
[[cos(phi), sin(phi), -x_x * sin(phi) + x_y * sin(phi)], [0, 0,
1]])
# TODO: Evaluate jacobian respect to line
H_line = np.array([[1, -x_x * sin(phi) + x_y * sin(phi)], [0, 1]])
return np.array([rho_, phi]), H_tf, H_line
def merge(lines, dist_thres, ang_thres):
'''
Merge similar lines according to the given thresholds.
'''
# No data received
if lines is None:
return np.array([])
# Check and merge similar consecutive lines
i = 0
while i in range(lines.shape[0]-1):
# Line angles
ang1 = math.atan2((lines[i,3]-lines[i,1]),(lines[i,2]-lines[i,0]))
ang2 = math.atan2((lines[i+1,3]-lines[i+1,1]),(lines[i+1,2]-lines[i+1,0]))
# Below thresholds?
angdiff = abs(angle_wrap(ang1 - ang2))
disdiff = math.sqrt(math.pow(lines[i+1,1]-lines[i,3],2) + math.pow(lines[i+1,0]-lines[i,2],2))
if angdiff < ang_thres and disdiff < dist_thres:
# Joined line
lines[i,:] = np.array([lines[i,0], lines[i,1], lines[i+1,2], lines[i+1,3]])
# Delete unnecessary line
lines = np.delete(lines, i+1, 0)
# Nothing to merge
else:
i += 1
print "============================================================"
print lines.shape
return lines
def merge(lines, dist_thres, ang_thres):
'''
Merge similar lines according to the given thresholds.
'''
# No data received
if lines is None:
return np.array([])
# Check and merge similar consecutive lines
i = 0
while i in range(lines.shape[0]-1):
# Line angles
ang1 = math.atan2(lines[i,3] - lines[i,1], lines[i,2] - lines[i,0])
ang2 = math.atan2(lines[i+1,3] - lines[i+1,1], lines[i+1,2] - lines[i+1,0])
# Below thresholds?
angdiff = abs(angle_wrap(ang1-ang2))
disdiff = math.sqrt(math.pow(lines[i,2]-lines[i+1,0],2) + math.pow(lines[i,3]-lines[i+1,1],2))
if angdiff < ang_thres and disdiff < dist_thres:
# Joined line
lines[i,:] = np.array([lines[i,0], lines[i,1], lines[i+1,2], lines[i+1,3]])
# Delete unnecessary line
lines = np.delete(lines, i+1,0)
# Nothing to merge
else:
i += 1
return lines
def get_polar_line(self, line, odom):
'''
Transforms a line from [x1 y1 x2 y2] from the world frame to the
vehicle frame using odometry [x y ang].
Returns [range theta]
'''
# Line points
x1 = line[0]
y1 = line[1]
x2 = line[2]
y2 = line[3]
# Compute line (a, b, c) and range
line = np.array([y1 - y2, x2 - x1, x1 * y2 - x2 * y1])
pt = np.array([odom[0], odom[1], 1])
dist = np.dot(pt, line) / np.linalg.norm(line[:2])
# Compute angle
if dist < 0:
ang = np.arctan2(line[1], line[0])
else:
ang = np.arctan2(-line[1], -line[0])
# Return in the vehicle frame
return np.array([np.abs(dist), angle_wrap(ang - odom[2])])
def read_position(self, msg):
'''
read_position copy the position received in the message (msg) to the
internal class variables self.position_x, self.position_y and
self.position_theta
Tip: in order to convert from quaternion to euler angles give a look
at tf.transformations
'''
#Find index of mobile_base
mobile_base_idx = -1
for i in range(len(msg.name)):
if msg.name[i] == 'mobile_base':
mobile_base_idx = i
break
#Update position_x and position_y
self.position_x = msg.pose[mobile_base_idx].position.x
self.position_y = msg.pose[mobile_base_idx].position.y
#Get euler angles from quaternion
(ang_x, ang_y, ang_z) = euler_from_quaternion([
msg.pose[mobile_base_idx].orientation.x,
msg.pose[mobile_base_idx].orientation.y,
msg.pose[mobile_base_idx].orientation.z,
msg.pose[mobile_base_idx].orientation.w
])
#Update theta with euler_z
self.position_theta = angle_wrap(ang_z)
def predict(self, odom):
'''
Moves particles with the given odometry.
odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame
'''
#Check if we have moved from previous reading.
if odom[0] == 0 and odom[1] == 0 and odom[2] == 0:
self.moving = False
else:
# TODO: code here!!
# Add Gaussian noise to odometry measures
xy_noise = np.random.randn(2, self.num) / 100
ang_noise = np.random.randn(self.num) / 100
# Increment particle positions in correct frame
cos_theta = np.cos(self.p_ang)
sin_theta = np.sin(self.p_ang)
dx = odom[0] * cos_theta - odom[1] * sin_theta
dy = odom[0] * sin_theta + odom[1] * cos_theta
self.p_xy += np.vstack((dx, dy)) + xy_noise
# Increment angle
self.p_ang += odom[2] + ang_noise
self.p_ang = angle_wrap(self.p_ang)
#Update flag for resampling
self.moving = True
def predict(self, odom):
'''
Moves particles with the given odometry.
odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame
'''
#Check if we have moved from previous reading.
if odom[0] == 0 and odom[1] == 0 and odom[2] == 0:
self.moving = False
else:
# TODO: code here!!
# Add Gaussian noise to odometry measures
delta_x_noisy = odom[0] + self.odom_lin_sigma * np.random.randn(
self.num)
delta_y_noisy = odom[1] + self.odom_lin_sigma * np.random.randn(
self.num)
delta_yaw_noisy = odom[2] + self.odom_ang_sigma * np.random.randn(
self.num)
# Transform from vehicle frame to world frame
delta_x_noisy_w = delta_x_noisy * np.cos(
-self.p_ang) + delta_y_noisy * np.sin(-self.p_ang)
delta_y_noisy_w = -delta_x_noisy * np.sin(
-self.p_ang) + delta_y_noisy * np.cos(-self.p_ang)
# Increment particle positions in correct frame
self.p_xy += np.vstack((delta_x_noisy_w, delta_y_noisy_w))
# Increment angle
self.p_ang += delta_yaw_noisy
self.p_ang = angle_wrap(self.p_ang)
#Update flag for resampling
self.moving = True
def predict(self, odom):
'''
Moves particles with the given odometry.
odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame
'''
#Check if we have moved from previous reading.
if odom[0] == 0 and odom[1] == 0 and odom[2] == 0:
self.moving = False
else:
for i in range(self.num):
self.p_xy[
0, i] += (odom[0] + np.random.randn() *
self.odom_lin_sigma) * np.cos(self.p_ang[i]) - (
odom[1] + np.random.randn() *
self.odom_lin_sigma) * np.sin(self.p_ang[i])
self.p_xy[
1, i] += (odom[0] + np.random.randn() *
self.odom_lin_sigma) * np.sin(self.p_ang[i]) + (
odom[1] + np.random.randn() *
self.odom_lin_sigma) * np.cos(self.p_ang[i])
self.p_ang += odom[2] * np.ones(self.num) + np.random.randn(
self.num) * self.odom_ang_sigma
self.p_ang = angle_wrap(self.p_ang)
#Update flag for resampling
self.moving = True
def predict(self, odom):
'''
Moves particles with the given odometry.
odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame
'''
#Check if we have moved from previous reading.
if odom[0] == 0 and odom[1] == 0 and odom[2] == 0:
self.moving = False
else:
# TODO: code here;!!
# Add Gaussian noise to odometry measures
odomx = odom[0] * np.cos(self.p_ang) - odom[1] * np.sin(self.p_ang)
odomy = odom[0] * np.sin(self.p_ang) + odom[1] * np.cos(self.p_ang)
odomx += self.odom_lin_sigma * np.random.randn(self.p_xy.shape[1])
odomy += self.odom_lin_sigma * np.random.randn(self.p_xy.shape[1])
odomt = odom[2] + self.odom_ang_sigma * np.random.randn(
self.p_ang.shape[0])
# Increment particle positions in correct frame
self.p_xy[0] += odomx
self.p_xy[1] += odomy
# Increment angle
self.p_ang += odomt
self.p_ang = angle_wrap(self.p_ang)
#Update flag for resampling
self.moving = True
def dist_to_goal_ang(self):
'''
dist_to_goal_ang computes the orientation distance to the active
goal
'''
return np.abs(
angle_wrap(self.theta[self.active_goal] - self.position_theta))
def compute_velocity(self):
'''
compute_velocity computes the velocity which will be published.
TODO implement!
'''
dx = self.x[
self.
active_goal] - self.position_x # distance in x between turtle and target
dy = self.y[
self.
active_goal] - self.position_y # distance in y between turtle and target
d = np.linalg.norm([dx,
dy]) # total distance between turtle and target
if d > self.goal_th_xy / 2: # Using threshold/2 to avoid entering and leaving goal radius
theta_target = np.arctan2(dy, dx) # angle to target
dt = angle_wrap(theta_target -
self.position_theta) # angle difference
# For derivative controller
d_deriv = d - self.d_prev
self.d_prev = d
dt_deriv = dt - self.dt_prev
self.dt_prev = dt
# Calculate control signals
linv = (self.kp_v * d + self.kd_v * d_deriv) * np.cos(dt / 2)
self.vmsg.linear.x = np.min([linv / np.sqrt(np.abs(linv)), 0.2])
angv = self.kp_w * dt + self.kd_w * dt_deriv
self.vmsg.angular.z = angv / np.sqrt(np.abs(angv))
elif not self.has_arrived_ang():
dt = angle_wrap(self.theta[self.active_goal] -
self.position_theta) # angle difference
# For derivative controller
dt_deriv = dt - self.dt_prev
self.dt_prev = dt
# Calculate control signals
self.vmsg.linear.x = 0
angv = self.kp_w * dt + self.kd_w * dt_deriv
self.vmsg.angular.z = angv / np.sqrt(np.abs(angv))
def weight(self, lines):
'''
Look for the lines seen from the robot and compare them to the given map.
Lines expressed as [x1 y1 x2 y2].
'''
# TODO: code here!!
# Constant values for all weightings
val_rng = 1.0 / (self.meas_rng_noise * np.sqrt(2 * np.pi))
val_ang = 1.0 / (self.meas_ang_noise * np.sqrt(2 * np.pi))
tunung_factor = 15
# Loop over particles
for i in range(self.num):
map_polar = np.zeros((self.map.shape[0],2))
# Transform map lines to local frame and to [range theta]
for j in range(self.map.shape[0]):
map_polar[j,:] = get_polar_line(self.map[j,:],[self.p_xy[0,i],self.p_xy[1,i],self.p_ang[i]])
# Transform measured lines to [range theta] and weight them
for j in range(lines.shape[0]):
if ( (lines[j,0]-lines[j,2])**2 + (lines[j,1]-lines[j,3])**2 ) > 0:
local_map_polar = get_polar_line(lines[j,:])
w = 0
index = -1
# Weight them
for k in range(map_polar.shape[0]):
w_r = np.exp(-(map_polar[k,0]-local_map_polar[0])**2/(tunung_factor*2*self.odom_lin_sigma**2)) * val_rng
w_a = np.exp(-angle_wrap(map_polar[k,1]-local_map_polar[1])**2/(tunung_factor*2*self.odom_ang_sigma**2)) * val_ang
# w = np.maximum(w,w_r*w_a)
if w_r*w_a > w:
w = w_r * w_a
index = k
# self.p_wei[i] *= w
# OPTIONAL question
# make sure segments correspond, if not put weight to zero
len_line_map = np.linalg.norm([(self.map[index,0]-self.map[index,2]), (self.map[index,1]-self.map[index,3])])
len_line_odom = np.linalg.norm([(lines[j,0]-lines[j,2]), (lines[j,1]-lines[j,3])])
# Check if measured line is larger than best fitting map line
# and set p_wei to 0 if it is (best match is invalid)
if len_line_odom > 1.1*len_line_map: # 10% error margin
# not 0 to avoid problems with turning all values to 0
self.p_wei[i] *= 0.00000000000000001
else:
self.p_wei[i] *= w
# Normalize weights
self.p_wei /= np.sum(self.p_wei)
# TODO: Compute efficient number
self.n_eff = 1.0/np.sum(self.p_wei**2)
def compute_velocity(self):
'''
compute_velocity computes the velocity which will be published.
'''
self.vmsg.angular.z = math.atan2(
self.y[self.active_goal] - self.position_y,
self.x[self.active_goal] - self.position_x) - self.position_theta
self.vmsg.angular.z = angle_wrap(self.vmsg.angular.z)
if (abs(self.vmsg.angular.z) > math.pi /
12): # in order not to enter an infinte loop of rotations
self.vmsg.linear.x = 0.3
else:
self.vmsg.linear.x = 0.5 * math.sqrt(
pow(self.position_x - self.x[self.active_goal], 2) +
pow(self.position_y - self.y[self.active_goal], 2))
if (self.has_arrived_xy()):
self.vmsg.linear.x = 0
self.vmsg.angular.z = angle_wrap(self.theta[self.active_goal] -
self.position_theta)
def tfPolarLine(self, tf, line):
'''
Transforms a polar line in the robot frame to a polar line in the
world frame
'''
# Decompose transformation
x_x = tf[0]
x_y = tf[1]
x_ang = tf[2]
# Compute the new phi
phi = angle_wrap(line[1] + x_ang)
# Auxiliar computations
sqrt2 = x_x**2 + x_y**2
sqrt = np.sqrt(sqrt2)
atan2 = np.arctan2(x_y, x_x)
sin = np.sin(atan2 - phi)
cos = np.cos(atan2 - phi)
# Compute the new rho
rho = line[0] + sqrt * cos
if rho < 0:
rho = -rho
phi = angle_wrap(phi + pi)
# Allocate jacobians
H_tf = np.zeros((2, 3))
H_line = np.eye(2)
a = np.cos(phi + x_ang)
b = np.sin(phi + x_ang)
c = -x_x * np.sin(phi + x_ang) + x_y * np.cos(phi + x_ang)
# TODO: Evaluate jacobian respect to transformation
H_tf = np.mat([[a, b, c], [0, 0, 1]])
# TODO: Evaluate jacobian respect to line
H_line = np.mat([[1, c], [0, 1]])
return np.array([rho, phi]), H_tf, H_line
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='/base_footprint',
parent='/world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# self.uk = np.array([delta_x,delta_y, angle_wrap(yaw)])
# Odometry seen from vehicle frame
self.uk = np.array([
delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)
])
# Flag available
self.cur_odom = msg
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
# Unlock thread
self.lock.release()
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag available
self.cur_odom = msg
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
# Unlock thread
self.lock.release()
def odom_callback(self, msg):
'''
Calculates the incremental odometry as seen from the vehicle frame.
'''
# Check if first data
if self.last_odom is None:
# Save
self.last_odom = msg
# Translation
self.trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
self.rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Calculate and publish increment
else:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Publish
odom = IncrementalOdometry2D()
odom.header.stamp = msg.header.stamp
odom.header.frame_id = '/estimated_position'
odom.delta_x = +delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw)
odom.delta_y = -delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw)
odom.delta_a = angle_wrap(yaw - lyaw)
self.pub_odom.publish(odom)
# For next loop
self.last_odom = msg
# World transform
self.tf_broad.sendTransform(translation=self.trans,
rotation=self.rot,
time=msg.header.stamp,
parent='/odom',
child='/world')
def merge(lines, dist_thres, ang_thres):
'''
Merge similar lines according to the given thresholds.
'''
# No data received
if lines is None: # or lines.shape[1] < 4:
return np.array([])
# if lines.shape[0]<2:
# return lines
# Check and merge similar consecutive lines
i = 0
while i < lines.shape[0] - 1:
# Line angles
ang1 = np.arctan2(lines[i][0] - lines[i][2], lines[i][1] - lines[i][3])
ang2 = np.arctan2(lines[i + 1][0] - lines[i + 1][2],
lines[i + 1][1] - lines[i + 1][3])
# Below thresholds?
angdiff = np.abs(angle_wrap(ang1 - ang2))
disdiff = np.linalg.norm([(lines[i][2] - lines[i + 1][0]),
(lines[i][3] - lines[i + 1][1])])
if angdiff < ang_thres and disdiff < dist_thres:
# Joined line
lines[i, :] = np.array(
[[lines[i][0], lines[i][1], lines[i + 1][2], lines[i + 1][3]]])
# Delete unnecessary line
lines = np.delete(lines, i + 1, 0)
# Nothing to merge
else:
i += 1
return lines
def compute_velocity(self):
'''
compute_velocity computes the velocity which will be published.
'''
self.vmsg=Twist()
#Compute the angle from turtlebot to the goal
angle_to_goal = math.atan2(self.y[self.active_goal]-self.position_y,self.x[self.active_goal]-self.position_x)
#Compute the angle difference between turtlebot orientation and the angle from turtlebot to the goal
ang_dif = angle_wrap(self.position_theta[2] - angle_to_goal)
#If turtlebot has not reach the goal position
if not self.has_arrived_xy():
self.vmsg.angular.z = -0.2*ang_dif
dis = math.sqrt(math.pow(self.y[self.active_goal] - self.position_y,2) + math.pow(self.x[self.active_goal] - self.position_x,2))
if dis > 0.5:
self.vmsg.linear.x = 0.1*dis
else:
self.vmsg.linear.x = 0.1
print ("distance to current goal:%f" %dis)
else:
self.vmsg.angular.z = 0.6 # When arriving the position, rotate in order to meet the angle restriction
print ("rotating")
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
# Calculate new state vector components
c = np.cos(self.xk[2])
s = np.sin(self.xk[2])
xr = self.xk[0] + uk[0] * c - uk[1] * s
yr = self.xk[1] + uk[0] * s + uk[1] * c
theta = funcs.angle_wrap(self.xk[2] + uk[2])
self.xk = np.array([xr, yr, theta]) # Should this be here?
# Calculate the current Ak matrix
A13 = -uk[0] * s - uk[1] * c
A23 = uk[0] * c - uk[1] * s
Ak = np.array([[1, 0, A13], [0, 1, A23], [0, 0, 1]])
# print(Ak)
# Calculate Wk
Wk = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
# Calculate Pk'
self.Pk = np.dot(Ak, np.dot(self.Pk, Ak.T)) + np.dot(
Wk, np.dot(self.Qk, Wk.T))
def merge(lines, dist_thres, ang_thres):
'''
Merge similar lines according to the given thresholds.
'''
# No data received
if lines is None:
return np.array([])
# Check and merge similar lines
i = 0
while i < lines.shape[0] - 1:
# Line angles
ang1 = np.arctan2(lines[i, 3] - lines[i, 1], lines[i, 2] - lines[i, 0])
ang2 = np.arctan2(lines[i + 1, 3] - lines[i + 1, 1],
lines[i + 1, 2] - lines[i + 1, 0])
# Below thresholds
ang = abs(angle_wrap(ang1 - ang2))
dis = np.sqrt( (lines[i,2] - lines[i+1,0])**2 + \
(lines[i,3] - lines[i+1,1])**2 )
if ang < ang_thres and dis < dist_thres:
# Joined line
lines[i, :] = [
lines[i, 0], lines[i, 1], lines[i + 1, 2], lines[i + 1, 3]
]
# Delete unnecessary line
lines = np.delete(lines, i + 1, axis=0)
# Nothing to merge
else:
i += 1
return lines
def compute_velocity(self):
'''
compute_velocity computes the velocity which will be published.
TODO implement!
'''
self.vmsg = Twist()
if self.dist_to_goal_xy() > self.goal_th_xy:
# Compute delta_angle
angle_to_goal = math.atan2(
self.y[self.active_goal] - self.position_y,
self.x[self.active_goal] - self.position_x)
delta_angle_to_goal = angle_wrap(self.position_theta -
angle_to_goal)
# Assign angular velocity as 2*delta_angle
self.vmsg.angular.z = -delta_angle_to_goal
# If delta_angle small (facing obstacle, we can move)
if np.abs(delta_angle_to_goal) < 0.05:
self.vmsg.linear.x = min(0.5, 2 * self.dist_to_goal_xy())
else:
self.vmsg.angular.z = -self.dist_to_goal_ang()
def dist_to_goal_ang(self):
'''
dist_to_goal_ang computes the orientation distance to the active
goal
'''
return np.abs(angle_wrap(self.theta[self.active_goal]-self.position_theta[2]))
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/sensor',
parent='/robot')
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/camera_depth_frame',
parent='/base_footprint')
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='/base_footprint',
parent='/odom')
self.tfBroad.sendTransform(
translation=(self.x0[0], self.x0[1], 0),
rotation=tf.transformations.quaternion_from_euler(
0, 0, self.x0[2]),
time=msg.header.stamp,
child='/odom',
parent='/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([
delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)
])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
def cbk_odom(self, msg):
"""Publish tf and calculate incremental odometry."""
# Save time
self.odomtime = msg.header.stamp
self.odom = msg
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='sensor',
parent='robot')
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='camera_depth_frame',
parent='base_footprint')
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='base_footprint',
parent='world')
self.tfBroad.sendTransform(translation=(0, 0, 0),
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='odom',
parent='world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - \
self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - \
self.last_odom.pose.pose.position.y
yaw = funcs.yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = funcs.yaw_from_quaternion(
self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x*np.cos(lyaw) + delta_y*np.sin(lyaw),
-delta_x*np.sin(lyaw) + delta_y*np.cos(lyaw),
funcs.angle_wrap(yaw - lyaw)])
# Flag available
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/sensor',
parent = '/robot')
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/camera_depth_frame',
parent = '/base_footprint')
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/odom')
self.tfBroad.sendTransform(translation = (self.x0[0],self.x0[1],0),
rotation = tf.transformations.quaternion_from_euler(0,0,self.x0[2]),
time = msg.header.stamp,
child = '/odom',
parent = '/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
- delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
