def PublishToGui(self, states_d, Input_to_Quad):
# WE ONLY PUBLIS TO TO GUI AT A CERTAIN FREQEUNCY
# WHICH IS NOT NECESSARILY THE FREQUENCY OF THE NODE
if self.PublishToGUI <= self.PublishToGUIBound:
# if we dont publish, we increase counter
self.PublishToGUI = self.PublishToGUI + 1
else:
# if we publish, we increase counter
self.PublishToGUI = 1
# create a message of type quad_state_and_cmd
st_cmd = quad_state_and_cmd()
# get current time
st_cmd.time = rospy.get_time()
# state of quad comes from QUALISYS, or other sensor
st_cmd.x = self.state_quad[0]
st_cmd.y = self.state_quad[1]
st_cmd.z = self.state_quad[2]
st_cmd.vx = self.state_quad[3]
st_cmd.vy = self.state_quad[4]
st_cmd.vz = self.state_quad[5]
st_cmd.roll = self.state_quad[6]
st_cmd.pitch = self.state_quad[7]
st_cmd.yaw = self.state_quad[8]
st_cmd.xd = states_d[0]
st_cmd.yd = states_d[1]
st_cmd.zd = states_d[2]
st_cmd.vxd = states_d[3]
st_cmd.vyd = states_d[4]
st_cmd.vzd = states_d[5]
st_cmd.cmd_1 = Input_to_Quad[0]
st_cmd.cmd_2 = Input_to_Quad[1]
st_cmd.cmd_3 = Input_to_Quad[2]
st_cmd.cmd_4 = Input_to_Quad[3]
st_cmd.cmd_5 = 1500.0
st_cmd.cmd_6 = 1500.0
st_cmd.cmd_7 = 1500.0
st_cmd.cmd_8 = 1500.0
self.pub.publish(st_cmd)
# controller state is supposed to be published
if self.flagPublish_ctr_st:
# publish controller state
msg = Controller_State()
msg.time = rospy.get_time()
msg.d_est = self.ControllerObject.d_est
self.pub_ctr_st.publish(msg)
def PublishToGui(self,states_d,Input_to_Quad):
# WE ONLY PUBLIS TO TO GUI AT A CERTAIN FREQEUNCY
# WHICH IS NOT NECESSARILY THE FREQUENCY OF THE NODE
if self.PublishToGUI <= self.PublishToGUIBound:
# if we dont publish, we increase counter
self.PublishToGUI = self.PublishToGUI + 1
else:
# if we publish, we increase counter
self.PublishToGUI = 1
# create a message of type quad_state_and_cmd
st_cmd = quad_state_and_cmd()
# get current time
st_cmd.time = rospy.get_time()
# state of quad comes from QUALISYS, or other sensor
st_cmd.x = self.state_quad[0]
st_cmd.y = self.state_quad[1]
st_cmd.z = self.state_quad[2]
st_cmd.vx = self.state_quad[3]
st_cmd.vy = self.state_quad[4]
st_cmd.vz = self.state_quad[5]
st_cmd.roll = self.state_quad[6]
st_cmd.pitch = self.state_quad[7]
st_cmd.yaw = self.state_quad[8]
st_cmd.xd = states_d[0]
st_cmd.yd = states_d[1]
st_cmd.zd = states_d[2]
st_cmd.vxd = states_d[3]
st_cmd.vyd = states_d[4]
st_cmd.vzd = states_d[5]
st_cmd.cmd_1 = Input_to_Quad[0]
st_cmd.cmd_2 = Input_to_Quad[1]
st_cmd.cmd_3 = Input_to_Quad[2]
st_cmd.cmd_4 = Input_to_Quad[3]
st_cmd.cmd_5 = 1500.0
st_cmd.cmd_6 = 1500.0
st_cmd.cmd_7 = 1500.0
st_cmd.cmd_8 = 1500.0
self.pub.publish(st_cmd)
# controller state is supposed to be published
if self.flagPublish_ctr_st:
# publish controller state
msg = Controller_State()
msg.time = rospy.get_time()
msg.d_est = self.controller.d_est
self.pub_ctr_st.publish(msg)
def PublishToGui(self):
# WE ONLY PUBLIS TO TO GUI AT A CERTAIN FREQEUNCY
# WHICH IS NOT NECESSARILY THE FREQUENCY OF THE NODE
if self.PublishToGUI <= self.PublishToGUIBound:
# if we dont publish, we increase counter
self.PublishToGUI = self.PublishToGUI + 1
else:
# if we publish, we increase counter
self.PublishToGUI = 1
# create a message of type quad_state_and_cmd
st_cmd = quad_state_and_cmd()
# get current time
st_cmd.time = rospy.get_time()
position_velocity = self.mission_object.get_pv()
# state of quad comes from QUALISYS, or other sensor
st_cmd.x = position_velocity[0]; st_cmd.y = position_velocity[1]; st_cmd.z = position_velocity[2]
st_cmd.vx = position_velocity[3]; st_cmd.vy = position_velocity[4]; st_cmd.vz = position_velocity[5]
euler_angle = self.mission_object.get_euler_angles()
st_cmd.roll = euler_angle[0]; st_cmd.pitch = euler_angle[1]; st_cmd.yaw = euler_angle[2]
ea_desired = self.mission_object.get_ea_desired()
st_cmd.roll_d = ea_desired[0]; st_cmd.pitch_d = ea_desired[1]; st_cmd.yaw_d = ea_desired[2]
position_velocity_desired = self.mission_object.get_pv_desired()
st_cmd.xd = position_velocity_desired[0]; st_cmd.yd = position_velocity_desired[1]; st_cmd.zd = position_velocity_desired[2]
st_cmd.vxd = position_velocity_desired[3]; st_cmd.vyd = position_velocity_desired[4]; st_cmd.vzd = position_velocity_desired[5]
rc_input_to_quad = self.mission_object.rc_output
st_cmd.cmd_1 = rc_input_to_quad[0]; st_cmd.cmd_2 = rc_input_to_quad[1]; st_cmd.cmd_3 = rc_input_to_quad[2]; st_cmd.cmd_4 = rc_input_to_quad[3]
st_cmd.cmd_5 = 1500.0; st_cmd.cmd_6 = 1500.0; st_cmd.cmd_7 = 1500.0; st_cmd.cmd_8 = 1500.0
self.pub.publish(st_cmd)
# controller state is supposed to be published
if self.flagPublish_ctr_st:
# publish controller state
msg = Controller_State()
msg.time = rospy.get_time()
msg.d_est = self.ControllerObject.d_est
self.pub_ctr_st.publish(msg)
def __init__(self, ns):
self.ns = ns
self.qualisys_check = False
self.qualisys_error = "NOT STARTED"
self.state = quad_state_and_cmd()
self.rc_check = False
self.rc_error = "NOT STARTED"
self.rc_msg = OverrideRCIn()
self.arm_check = False
self.arm_error = False
self.mode_check = False
self.mode_error = False
self.traj_check = False
self.traj_error = False
self.traj_point = None
self.namespace = "/" + self.ns + "/"
self.connect_to_ROS()
def __init__(self,ns): self.ns = ns self.qualisys_check = False self.qualisys_error = "NOT STARTED" self.state = quad_state_and_cmd() self.rc_check = False self.rc_error = "NOT STARTED" self.rc_msg = OverrideRCIn() self.arm_check = False self.arm_error = False self.mode_check = False self.mode_error = False self.traj_check = False self.traj_error = False self.traj_point = None self.namespace = "/" + self.ns + "/" self.connect_to_ROS()
def PublishToGui(self):
# WE ONLY PUBLIS TO TO GUI AT A CERTAIN FREQEUNCY
# WHICH IS NOT NECESSARILY THE FREQUENCY OF THE NODE
if self.PublishToGUI <= self.PublishToGUIBound:
# if we dont publish, we increase counter
self.PublishToGUI = self.PublishToGUI + 1
else:
# if we publish, we increase counter
self.PublishToGUI = 1
# create a message of type quad_state_and_cmd
st_cmd = quad_state_and_cmd()
# get current time
st_cmd.time = rospy.get_time()
position_velocity = self.mission_object.get_pv()
# state of quad comes from QUALISYS, or other sensor
st_cmd.x = position_velocity[0]
st_cmd.y = position_velocity[1]
st_cmd.z = position_velocity[2]
st_cmd.vx = position_velocity[3]
st_cmd.vy = position_velocity[4]
st_cmd.vz = position_velocity[5]
euler_angle = self.mission_object.get_euler_angles()
st_cmd.roll = euler_angle[0]
st_cmd.pitch = euler_angle[1]
st_cmd.yaw = euler_angle[2]
ea_desired = self.mission_object.get_ea_desired()
st_cmd.roll_d = ea_desired[0]
st_cmd.pitch_d = ea_desired[1]
st_cmd.yaw_d = ea_desired[2]
position_velocity_desired = self.mission_object.get_pv_desired()
st_cmd.xd = position_velocity_desired[0]
st_cmd.yd = position_velocity_desired[1]
st_cmd.zd = position_velocity_desired[2]
st_cmd.vxd = position_velocity_desired[3]
st_cmd.vyd = position_velocity_desired[4]
st_cmd.vzd = position_velocity_desired[5]
rc_input_to_quad = self.mission_object.rc_output
st_cmd.cmd_1 = rc_input_to_quad[0]
st_cmd.cmd_2 = rc_input_to_quad[1]
st_cmd.cmd_3 = rc_input_to_quad[2]
st_cmd.cmd_4 = rc_input_to_quad[3]
st_cmd.cmd_5 = 1500.0
st_cmd.cmd_6 = 1500.0
st_cmd.cmd_7 = 1500.0
st_cmd.cmd_8 = 1500.0
self.pub.publish(st_cmd)
# controller state is supposed to be published
if self.flagPublish_ctr_st:
# publish controller state
msg = Controller_State()
msg.time = rospy.get_time()
msg.d_est = self.ControllerObject.d_est
self.pub_ctr_st.publish(msg)
