def odometry_callback(self, msg):
"""Handle odometry callback: The actual control loop."""
# Update local planner's vehicle position and orientation
pos = [
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
]
quat = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
]
self.local_planner.update_vehicle_pose(pos, quat)
# Compute the desired position
t = rospy.Time.now().to_sec()
des = self.local_planner.interpolate(t)
# Publish the reference
ref_msg = TrajectoryPoint()
ref_msg.header.stamp = rospy.Time.now()
ref_msg.header.frame_id = self.local_planner.inertial_frame_id
ref_msg.pose.position = geometry_msgs.Vector3(*des.p)
ref_msg.pose.orientation = geometry_msgs.Quaternion(*des.q)
ref_msg.velocity.linear = geometry_msgs.Vector3(*des.vel[0:3])
ref_msg.velocity.angular = geometry_msgs.Vector3(*des.vel[3::])
self.reference_pub.publish(ref_msg)
p = self.vector_to_np(msg.pose.pose.position)
forward_vel = self.vector_to_np(msg.twist.twist.linear)
ref_vel = des.vel[0:3]
q = self.quaternion_to_np(msg.pose.pose.orientation)
rpy = trans.euler_from_quaternion(q, axes='sxyz')
# Compute tracking errors wrt world frame:
e_p = des.p - p
abs_pos_error = np.linalg.norm(e_p)
abs_vel_error = np.linalg.norm(ref_vel - forward_vel)
# Generate error message
error_msg = TrajectoryPoint()
error_msg.header.stamp = rospy.Time.now()
error_msg.header.frame_id = self.local_planner.inertial_frame_id
error_msg.pose.position = geometry_msgs.Vector3(*e_p)
error_msg.pose.orientation = geometry_msgs.Quaternion(
*trans.quaternion_multiply(trans.quaternion_inverse(q), des.q))
error_msg.velocity.linear = geometry_msgs.Vector3(
*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear)))
error_msg.velocity.angular = geometry_msgs.Vector3(
*(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular)))
# Based on position tracking error: Compute desired orientation
pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2]))
# Limit desired pitch angle:
pitch_des = max(-self.desired_pitch_limit,
min(pitch_des, self.desired_pitch_limit))
yaw_des = math.atan2(e_p[1], e_p[0])
yaw_err = self.unwrap_angle(yaw_des - rpy[2])
# Limit yaw effort
yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit,
yaw_err))
# Roll: P controller to keep roll == 0
roll_control = self.p_roll * rpy[0]
# Pitch: P controller to reach desired pitch angle
pitch_control = self.p_pitch * self.unwrap_angle(
pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] -
msg.twist.twist.angular.y)
# Yaw: P controller to reach desired yaw angle
yaw_control = self.p_yaw * yaw_err + self.d_yaw * (
des.vel[5] - msg.twist.twist.angular.z)
# Limit thrust
thrust = min(
self.max_thrust,
self.p_gain_thrust * np.linalg.norm(abs_pos_error) +
self.d_gain_thrust * abs_vel_error)
thrust = max(self.min_thrust, thrust)
rpy = np.array([roll_control, pitch_control, yaw_control])
# In case the world_ned reference frame is used, the convert it back
# to the ENU convention to generate the reference fin angles
rtf = deepcopy(self.rpy_to_fins)
if self.local_planner.inertial_frame_id == 'world_ned':
rtf[:, 1] *= -1
rtf[:, 2] *= -1
# Transform orientation command into fin angle set points
fins = rtf.dot(rpy)
# Check for saturation
max_angle = max(np.abs(fins))
if max_angle >= self.max_fin_angle:
fins = fins * self.max_fin_angle / max_angle
thrust_force = self.thruster.tam_column * thrust
self.thruster.publish_command(thrust_force[0])
cmd = FloatStamped()
for i in range(self.n_fins):
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = '%s/fin%d' % (self.namespace, i)
cmd.data = min(fins[i], self.max_fin_angle)
cmd.data = max(cmd.data, -self.max_fin_angle)
self.pub_cmd[i].publish(cmd)
self.error_pub.publish(error_msg)
def publish_command(self, delta):
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.data = delta
self.pub.publish(msg)
def publish_command(self, thrust):
self._update(thrust)
output = FloatStamped()
output.header.stamp = rospy.Time.now()
output.data = self._command
self._command_pub.publish(output)