def vehicle_odom_callback(self, data, vehicle_number):
p_enu = np.array([data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z])
p_ned = transform_position_I(p_enu)
v_enu = np.array([data.twist.twist.linear.x,
data.twist.twist.linear.y,
data.twist.twist.linear.z])
v_ned = transform_position_I(v_enu)
q_enu = np.array([data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w])
q_ned = transform_orientation_I(transform_orientation_B(Rotation.from_quat(q_enu))).as_quat()
R_ned = transform_orientation_I(transform_orientation_B(Rotation.from_quat(q_enu))).as_dcm()
o_enu = np.array([data.twist.twist.angular.x,
data.twist.twist.angular.y,
data.twist.twist.angular.z])
o_ned = transform_omega_B(o_enu)
self.pQ[vehicle_number] = p_ned.reshape([3,1])
self.vQ[vehicle_number] = v_ned.reshape([3,1])
self.RQ[vehicle_number] = R_ned.reshape([3,3])
self.oQ[vehicle_number] = o_ned.reshape([3,1])
if not self.sub_topics_ready['vehicle_odom']:
self.sub_topics_ready['vehicle_odom'] = True
def load_odom_callback(self, data):
p_enu = np.array([data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z])
p_ned = transform_position_I(p_enu)
v_enu = np.array([data.twist.twist.linear.x,
data.twist.twist.linear.y,
data.twist.twist.linear.z])
v_ned = transform_position_I(v_enu)
q_enu = np.array([data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w])
q_ned = transform_orientation_I(transform_orientation_B(Rotation.from_quat(q_enu))).as_quat()
R_ned = transform_orientation_I(transform_orientation_B(Rotation.from_quat(q_enu))).as_dcm()
o_enu = np.array([data.twist.twist.angular.x,
data.twist.twist.angular.y,
data.twist.twist.angular.z])
o_ned = transform_omega_B(o_enu)
self.pL = p_ned.reshape([1,3,1])
self.vL = v_ned.reshape([1,3,1])
self.RL = R_ned.reshape([1,3,3])
self.oL = o_ned.reshape([1,3,1])
if not self.sub_topics_ready['load_odom']:
self.sub_topics_ready['load_odom'] = True
def send_att(self):
try:
rate = rospy.Rate(50) # Hz
self.att.body_rate = Vector3()
self.att.header = Header()
self.att.header.frame_id = "base_footprint"
self.att.orientation = Quaternion(*self.q_input[0])
self.att.thrust = self.T_input[0]
self.att.type_mask = 7 # ignore body rate
# print('self.att.target_system:', self.att.target_system)
# print('self.att.target_component:', self.att.target_component)
except Exception as e:
print(e)
pass
while not rospy.is_shutdown():
self.att.header.stamp = rospy.Time.now()
if (not self.position_active):
self.computeSystemStates()
if (self.attitude_active):
(T, q) = self.controller_attitude()
if (self.load_active):
(T, q) = self.controller_load()
for i in range(self.n):
q_ENU = transform_orientation_I(
transform_orientation_B(
Rotation.from_quat(q))).as_quat()
self.att.orientation = Quaternion(*q_ENU[i])
self.att.thrust = T[i]
try:
self.att_setpoint_pub[i].publish(self.att)
except rospy.exceptions.ROSException as e:
print(f"Vehicle {i} - Attitude: ", e)
try: # prevent garbage in console output when thread is killed
rate.sleep()
# time.sleep(0.001)
except rospy.ROSInterruptException:
pass
def send_pos(self):
rate = rospy.Rate(10) # Hz
self.pos.header = Header()
self.pos.header.frame_id = "base_footprint"
while not rospy.is_shutdown():
self.pos.header.stamp = rospy.Time.now()
# while True:
t = rospy.get_time()
self.pd = np.array([
cfg[i]['starting_position']
for i in range(cfg['number_of_vehicles'])
]).reshape(cfg['number_of_vehicles'], cfg['dimensions'], 1)
self.Dpd = 0 * self.pd
self.D2pd = 0 * self.pd
if (self.position_active):
pd = self.controller_position()
pd_ENU = transform_position_I(pd)
# set a position setpoint
self.pos.pose.position.x = pd_ENU[0]
self.pos.pose.position.y = pd_ENU[1]
self.pos.pose.position.z = pd_ENU[2]
# set yaw angle
yaw_degrees = 0
yaw = math.radians(yaw_degrees)
quaternion = transform_orientation_I(
transform_orientation_B(
(Rotation.from_euler('zyx', [yaw, 0, 0])))).as_quat()
self.pos.pose.orientation = Quaternion(*quaternion)
for i in range(self.n):
try:
self.pos_setpoint_pub[i].publish(self.pos)
except rospy.exceptions.ROSException as e:
print(e)
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass