def updateImu(self, imu):
self.imu_last_update = rospy.Time.now()
self.imu_init = True
if not self.imu_data :
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = self.imu_tf.M * imu_data
angle = imu_data.GetRPY()
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
self.imu_data = True
else:
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = self.imu_tf.M * imu_data
angle = imu_data.GetRPY()
angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
self.odom.pose.pose.orientation.x = angle_quaternion[0]
self.odom.pose.pose.orientation.y = angle_quaternion[1]
self.odom.pose.pose.orientation.z = angle_quaternion[2]
self.odom.pose.pose.orientation.w = angle_quaternion[3]
# Derive angular velocities from orientations #######################
period = (imu.header.stamp - self.last_imu_update).to_sec()
self.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - self.last_imu_orientation[0]) / period
self.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - self.last_imu_orientation[1]) / period
self.odom.twist.twist.angular.z = cola2_lib.normalizeAngle(angle[2] - self.last_imu_orientation[2]) / period
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
#####################################################################
if self.makePrediction():
self.ekf.updatePrediction()
self.publishData()
def updateImu(self, imu):
config = self.config
config.imu_last_update = imu.header.stamp #rospy.Time.now()
config.imu_init = True
if not config.imu_data :
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = config.imu_tf.M * imu_data
angle = imu_data.GetRPY()
config.last_imu_orientation = angle
config.last_imu_update = imu.header.stamp
# Initialize heading buffer in order to apply a savitzky_golay derivation
if len(config.heading_buffer) == 0:
config.heading_buffer.append(angle[2])
inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
config.heading_buffer.append(config.heading_buffer[-1] + inc)
if len(config.heading_buffer) == len(config.savitzky_golay_coeffs):
config.imu_data = True
else:
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = config.imu_tf.M * imu_data
angle = imu_data.GetRPY()
angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
config.odom.pose.pose.orientation.x = angle_quaternion[0]
config.odom.pose.pose.orientation.y = angle_quaternion[1]
config.odom.pose.pose.orientation.z = angle_quaternion[2]
config.odom.pose.pose.orientation.w = angle_quaternion[3]
# Derive angular velocities from orientations #######################
period = (imu.header.stamp - config.last_imu_update).to_sec()
# For yaw rate we apply a savitzky_golay derivation
inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
config.heading_buffer.append(config.heading_buffer[-1] + inc)
config.heading_buffer.pop(0)
config.odom.twist.twist.angular.z = convolve(config.heading_buffer, config.savitzky_golay_coeffs, mode='valid') / period
# TODO: Roll rate and Pitch rate should be also savitzky_golay derivations
config.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - config.last_imu_orientation[0]) / period
config.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - config.last_imu_orientation[1]) / period
config.last_imu_orientation = angle
config.last_imu_update = imu.header.stamp
#####################################################################
try:
self.LOCK.acquire()
if self.makePrediction(imu.header.stamp):
self.filter.updatePrediction()
self.publishData()
finally:
self.LOCK.release()
def moveMode_X_Y_Z_YAW(self, req, now):
tolerance = [req.position_tolerance.x,
req.position_tolerance.y,
req.position_tolerance.z,
req.orientation_tolerance.roll,
req.orientation_tolerance.pitch,
req.orientation_tolerance.yaw]
z_error = 0.0
yaw_error = 0.0
x_error = 0.0
y_error = 0.0
# Compute Errors
# Altitude mode
if req.altitude_mode :
z_error = self.altitude - req.altitude
desired = [req.position.north,
req.position.east,
req.altitude,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = [self.p[0],
self.p[1],
self.altitude,
self.p[3],
self.p[4],
self.p[5]]
# Depth Mode
else :
z_error = req.position.depth - self.p[2]
desired = [req.position.north,
req.position.east,
req.position.depth,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = self.p
yaw_error = cola2_lib.normalizeAngle(req.orientation.yaw - self.p[5])
[x_error, y_error] = self.computePosError(self.p[0],
self.p[1],
self.p[5],
req.position.north,
req.position.east)
error = array([x_error, y_error, z_error, 0.0, 0.0, yaw_error])
#Compute Body Velocity request
bvr = zeros(6)
real_period = (now - self.past_time) #nano seconds to seconds
self.past_time = now
[bvr, self.ek_1, self.eik_1] = self.pid_x_y_z_yaw.computePid(error,
zeros(6),
self.ek_1,
self.eik_1,
real_period)
# Send Body Velocity Request
if req.disable_axis.z == True:
bvr[2] = 0.0
if req.disable_axis.yaw == True:
bvr[5] = 0.0
success = self.checkTolerance([False, False, req.disable_axis.z, True, True, req.disable_axis.yaw],
current,
desired,
tolerance)
print 'current: ', current
print 'desired: ', desired
print 'tolerance: ', tolerance
return [success, bvr*self.max_velocity]
def moveMode_LOS(self, previous_req, req, now):
z_error = 0.0
yaw_error = 0.0
tolerance = [req.position_tolerance.x,
req.position_tolerance.y,
req.position_tolerance.z,
req.orientation_tolerance.roll,
req.orientation_tolerance.pitch,
req.orientation_tolerance.yaw]
# Compute Error
# Altitude mode
if req.altitude_mode :
#TODO: Hem falta llegir la current altitude (hauria de ser ella enlloc de _p[2])!!!
z_error = self.altitude - req.altitude
desired = [req.position.north,
req.position.east,
req.altitude,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = [self.p[0],
self.p[1],
self.altitude,
self.p[3],
self.p[4],
self.p[5]]
# Depth Mode
else :
z_error = req.position.depth - self.p[2]
desired = [req.position.north,
req.position.east,
req.position.depth,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = self.p
#Compute cross-track error
alpha = math.atan2(req.position.east - previous_req.position.east, req.position.north - previous_req.position.north)
#print "Alpha: " + str(alpha)
e = -(self.p[0] - previous_req.position.north)*sin(alpha) + (self.p[1] - previous_req.position.east)*cos(alpha)
#print "e:" + str(e)
beta = math.atan2(self.v[1], self.v[0])
#print "beta: " + str(beta)
delta = 4.0
#print "cross track error: " + str(math.atan(-e/delta))
yaw_error = self.p[5] - (alpha + math.atan(-e/delta)) + beta
yaw_error = cola2_lib.normalizeAngle(yaw_error)
#print "yaw_error: " + str(yaw_error)
error = array([0.0, 0.0, z_error, 0.0, 0.0, -yaw_error])
#Compute Body Velocity request
bvr = zeros(6)
real_period = (now - self.past_time) #nano seconds to seconds
self.past_time = now
[bvr, self.ek_1, self.eik_1] = self.pid_x_z_yaw.computePid(error,
zeros(6),
self.ek_1,
self.eik_1,
real_period)
bvr *= self.max_velocity
#TODO: All these vars have to be defined in some other place
#Set x velocity to a constant value that depends on the yaw_error
min_yaw_error = 0.05 #6 degrees
max_yaw_error = self.max_angle_error
min_x_v = self.min_velocity_los[0]
max_x_v = self.max_velocity[0]
SURGE_DEPEND_ON_YAW = False
if SURGE_DEPEND_ON_YAW:
error_angle = atan2(max_x_v - min_x_v, max_yaw_error - min_yaw_error)
if abs(yaw_error) < min_yaw_error: bvr[0] = max_x_v
elif abs(yaw_error) > max_yaw_error: bvr[0] = min_x_v
else: bvr[0] = min_x_v + tan(error_angle)*(max_yaw_error - abs(yaw_error))
else:
bvr[0] = max_x_v
#When the vehicle reaches or leaves a way-point the forward velocity follow a trapezoid
#from min_distance to 0.0 distance respect to the way-point, the forward velocity decreases linearly
#from 100% of the computed value to min_tant_per_one*100 % of it.
#TODO: All these vars have to be defined in some other place
min_tant_per_one = 0.10 #tant per one
min_distance = 5.0 #meters, this value has to be bigger than the acceptance sphere
dist_angle = atan2(1-min_tant_per_one, min_distance)
#distance to current way-point
distance_current = sqrt((req.position.north-self.p[0])**2 + (req.position.east-self.p[1])**2)
#distance to previous way-point
distace_previous = sqrt((previous_req.position.north-self.p[0])**2 + (previous_req.position.east-self.p[1])**2)
#Take the smaller one
distance = min(distance_current, distace_previous)
if distance < min_distance:
tant_per_one = min_tant_per_one + tan(dist_angle)*distance
bvr[0] *= tant_per_one
if bvr[0] < min_x_v: bvr[0] = min_x_v
# Compute feedback
success = self.checkTolerance([False, False, req.disable_axis.z, True, True, True],
current,
desired,
tolerance)
#Check if the limit line has been crossed. Where the limit line is the line perpendicular to
#the desired path that pass through the current way-point,
inc_y = req.position.east - previous_req.position.east
if inc_y != 0.0 :
m_l = -(req.position.north - previous_req.position.north) / inc_y
else:
m_l = 999999.9
c_l = -m_l * req.position.north + req.position.east
current_d = (m_l * self.p[0] - self.p[1] + c_l)/sqrt(m_l**2 + 1)
signe = (m_l * previous_req.position.north - previous_req.position.east + c_l)/sqrt(m_l**2 + 1)
if signe * current_d < 0.0:
success = True
# print "distance: ", current_d
# print "signe: ", signe
return [success, bvr]
def moveMode_X_Z_YAW(self, req, now):
x_error = 0.0
z_error = 0.0
yaw_error = 0.0
tolerance = [req.position_tolerance.x,
req.position_tolerance.y,
req.position_tolerance.z,
req.orientation_tolerance.roll,
req.orientation_tolerance.pitch,
req.orientation_tolerance.yaw]
# Compute Error
# Altitude mode
if req.altitude_mode :
#TODO: Hem falta llegir la current altitude (hauria de ser ella enlloc de _p[2])!!!
z_error = self.altitude - req.altitude
desired = [req.position.north,
req.position.east,
req.altitude,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = [self.p[0],
self.p[1],
self.altitude,
self.p[3],
self.p[4],
self.p[5]]
# Depth Mode
else :
z_error = req.position.depth - self.p[2]
desired = [req.position.north,
req.position.east,
req.position.depth,
req.orientation.roll,
req.orientation.pitch,
req.orientation.yaw]
current = self.p
inc_x = req.position.north - self.p[0]
inc_y = req.position.east - self.p[1]
desired_yaw = math.atan2(inc_y, inc_x)
yaw_error = cola2_lib.normalizeAngle(desired_yaw - self.p[5])
if abs(yaw_error) < self.max_angle_error :
x_error = math.sqrt(pow(inc_x,2) + pow(inc_y,2))
error = array([x_error, 0.0, z_error, 0.0, 0.0, yaw_error])
#Compute Body Velocity request
bvr = zeros(6)
real_period = (now - self.past_time) #nano seconds to seconds
self.past_time = now
[bvr, self.ek_1, self.eik_1] = self.pid_x_z_yaw.computePid(error,
zeros(6),
self.ek_1,
self.eik_1,
real_period)
# Compute feedback
success = self.checkTolerance([False, False, req.disable_axis.z, True, True, True],
current,
desired,
tolerance)
# When moving with waypoint mode (normaly short distances) reduce the surge velocity
max = list(self.max_velocity)
max[0] = max[0] / 2.0
return [success, bvr * max]