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)