18 from tf_quaternion.transformations
import quaternion_matrix
19 from uuv_gazebo_ros_plugins_msgs.msg
import FloatStamped
28 self.
rot = quaternion_matrix(quat)[0:3, 0:3]
30 unit_z = self.
rot[:, 2]
32 surge_vel = np.array([1, 0, 0])
33 fin_surge_vel = surge_vel - np.dot(surge_vel, unit_z) / np.linalg.norm(unit_z)**2 * unit_z
35 self.
lift_vector = -1 * np.cross(unit_z, fin_surge_vel) / np.linalg.norm(np.cross(unit_z, fin_surge_vel))
38 self.
pub = rospy.Publisher(self.
topic, FloatStamped, queue_size=3)
42 msg.header.stamp = rospy.Time.now()
def __init__(self, index, pos, quat, topic)
def publish_command(self, delta)