class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow):
def __init__(self):
super(Gui2Ros, self).__init__()
self.setupUi(self)
self.map = 'indoor1'
self.comboBox_maps.currentIndexChanged.connect(self.initplot)
self.button_run.clicked.connect(self.startrun)
self.close_flag = False
self.local_pose = PoseStamped()
self.local_vel = Twist()
self.m = PlotCanvas(self, self.map)
self.m.move(180, 0)
self.flag = 0
# rospy.init_node('multirotor_pyqt5_control')
def initplot(self):
self.map = self.comboBox_maps.currentText()
self.m.canvas_update(self.map)
def startrun(self):
print 'start run!'
self.init_controller()
self.pSend2ros = Process(target=self.run_process)
self.pSend2ros.start()
self.text_thread = Ros2Gui(self.multirotor_select, self.multirotor_num, self.multi_type)
self.text_thread.update_text.connect(self.display)
self.text_thread.plot_array.connect(self.plot)
self.text_thread.start()
# self.pSend2ros = Process(target=self.run_process)
# self.pSend2ros.start()
def init_controller(self):
self.text_show_info.setPlainText('data')
self.multi_num = 0
self.multi_type = []
counnnt = 0
print self.multirotor_select
for j in self.multirotor_select:
self.multi_num = self.multi_num + self.multirotor_num[j]
for id_1 in range(self.multirotor_num[j]):
self.multi_type.append(self.multirotor_type[j])
counnnt+=1
self.color_plot = ['' for i in range(self.multi_num)]
for i in range(self.multi_num):
color_R = hex(random.randint(16,255))
color_G = hex(random.randint(16,255))
color_B = hex(random.randint(16,255))
self.color_plot[i] = '#'+str(color_R)+str(color_G)+str(color_B)
self.color_plot[i] = self.color_plot[i].replace('0x','')
#publish messages to ros nodes like a keyboard
def run_process(self):
rospy.init_node('multirotor_pyqt5_control')
counnnt = 0
if self.control_type == 'vel':
self.multi_cmd_vel_flu_pub = [None] * self.multi_num
self.multi_cmd_pub = [None] * self.multi_num
for i in self.multirotor_select:
for k in range(self.multirotor_num[i]):
if i == 7:
self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher('/ugv_' + str(k) + '/cmd_vel', Twist, queue_size=10)
self.multi_cmd_pub[counnnt] = rospy.Publisher('/ugv_' + str(k) + '/cmd', String,queue_size=10)
else:
self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd_vel_flu', Twist, queue_size=10)
self.multi_cmd_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd', String,queue_size=10)
counnnt += 1
self.leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
else:
self.multi_cmd_accel_flu_pub = [None] * self.multi_num
self.multi_cmd_pub = [None] * self.multi_num
for i in self.multirotor_select:
for k in range(self.multirotor_num[i]):
self.multi_cmd_accel_flu_pub[i] = rospy.Publisher(
'/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd_accel_flu', Twist, queue_size=10)
self.multi_cmd_pub[i] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd',
String,
queue_size=10)
counnnt = 0
self.leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
self.twist = [Twist() for i in range (self.multi_num)]
self.cmd = ['' for i in range (self.multi_num)]
self.ctrl_leader = True
self.cmd_vel_mask = False
for j in range(self.multi_num):
self.twist[j].angular.x = 0.0
self.twist[j].angular.y = 0.0
last_forward = [0.0 for i in range(self.multi_num)]
last_upward = [0.0 for i in range(self.multi_num)]
last_leftward = [0.0 for i in range(self.multi_num)]
last_orientation = [0.0 for i in range(self.multi_num)]
last_ctrl_leader = False
last_cmd_vel_mask = False
last_multirotor_get_control = [0 for i in range(self.multi_num)]
last_forward_all = 0.0
last_upward_all = 0.0
last_leftward_all = 0.0
last_orientation_all = 0.0
num = 0
rate = rospy.Rate(30)
check_stop_flag = False
print('StartRun!')
start_flag = False
flag = False
time = 0
while True:
if not start_flag:
flag = self.q_start_control_flag.get()
if flag:
time += 1
start_flag = True
num += 1
if self.q_multirotor_get_control.empty():
multirotor_get_control = last_multirotor_get_control
else:
multirotor_get_control = self.q_multirotor_get_control.get()
last_multirotor_get_control = multirotor_get_control
if self.q_forward.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.x = last_forward[i]
else:
forward = self.q_forward.get()
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.x = forward
last_forward[i] = self.twist[i].linear.x
if self.q_upward.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.z = last_upward[i]
else:
upward = self.q_upward.get()
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.z = upward
last_upward[i] = self.twist[i].linear.z
if self.q_leftward.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.y = last_leftward[i]
else:
leftward = self.q_leftward.get()
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].linear.y = leftward
last_leftward[i] = self.twist[i].linear.y
if self.q_orientation.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].angular.z = last_orientation[i]
else:
orientation = self.q_orientation.get()
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.twist[i].angular.z = orientation
last_orientation[i] = self.twist[i].angular.z
if self.q_ctrl_leader.empty():
self.ctrl_leader = last_ctrl_leader
else:
self.ctrl_leader = self.q_ctrl_leader.get()
last_ctrl_leader = self.ctrl_leader
if self.q_cmd.empty():
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.cmd[i] = ''
else:
cmd = self.q_cmd.get()
if self.ctrl_leader:
for i in range(self.multi_num):
if i == 1:
self.cmd[i] = cmd
else:
self.cmd[i] = ''
else:
for i in range(self.multi_num):
if multirotor_get_control[i]:
self.cmd[i] = cmd
print(self.cmd[i])
if self.q_cmd_vel_mask.empty():
self.cmd_vel_mask = last_cmd_vel_mask
else:
self.cmd_vel_mask = self.q_cmd_vel_mask.get()
last_cmd_vel_mask = self.cmd_vel_mask
if self.q_stop_flag.empty():
pass
else:
check_stop_flag = self.q_stop_flag.get()
if check_stop_flag:
for i in range(self.multi_num):
self.cmd[i] = 'AUTO.RTL'
if self.ctrl_leader:
if self.control_type == 'vel':
self.leader_cmd_vel_flu_pub.publish(self.twist[1])
else:
self.leader_cmd_accel_flu_pub.publish(self.twist[1])
self.leader_cmd_pub.publish(self.cmd[1])
print self.cmd[1]
else:
for i in range(self.multi_num):
if not self.cmd_vel_mask:
if self.control_type == 'vel':
self.multi_cmd_vel_flu_pub[i].publish(self.twist[i])
else:
self.multi_cmd_accel_flu_pub[i].publish(self.twist[i])
self.multi_cmd_pub[i].publish(self.cmd[i])
# print self.cmd[0]
else:
print 'shut down!'
rate.sleep()
if check_stop_flag:
self.q_stop_flag.put(True)
rospy.signal_shutdown('STOP!')
break
def display(self, data):
self.text_show_info.setPlainText(data)
def plot(self, data):
for i in range(self.multi_num):
self.m.ax.plot(data[i][0], data[i][1], color = self.color_plot[i])
# self.m.canvas_update(self.map)
self.m.draw()
class Gui2Ros(QMainWindow, xtd_ui.Ui_MainWindow):
def __init__(self):
super(Gui2Ros, self).__init__()
self.setupUi(self)
self.map = 'indoor1'
self.comboBox_maps.currentIndexChanged.connect(self.initplot)
self.button_run.clicked.connect(self.startrun)
self.cmd = ''
self.ctrl_leader = True
self.cmd_vel_mask = False
self.close_flag = False
self.local_pose = PoseStamped()
self.local_vel = Twist()
self.m = PlotCanvas(self, self.map)
self.m.move(180, 0)
def initplot(self):
self.map = self.comboBox_maps.currentText()
self.m.canvas_update(self.map)
def startrun(self):
print 'start run!'
self.color_plot = ['' for i in range(self.multirotor_num)]
for i in range(self.multirotor_num):
color_R = hex(random.randint(16, 255))
color_G = hex(random.randint(16, 255))
color_B = hex(random.randint(16, 255))
self.color_plot[i] = '#' + str(color_R) + str(color_G) + str(
color_B)
self.color_plot[i] = self.color_plot[i].replace('0x', '')
self.pSend2ros = Process(target=self.run_process)
self.pSend2ros.start()
self.text_thread = Ros2Gui(self.multirotor_num, self.multirotor_type)
self.text_thread.update_text.connect(self.display)
self.text_thread.plot_array.connect(self.plot)
self.text_thread.start()
#publish messages to ros nodes like a keyboard
def run_process(self):
rospy.init_node('multirotor_pyqt5_control')
self.text_show_info.setPlainText('data')
if self.control_type == 'vel':
self.multi_cmd_vel_flu_pub = [None] * self.multirotor_num
self.multi_cmd_pub = [None] * self.multirotor_num
for i in range(self.multirotor_num):
self.multi_cmd_vel_flu_pub[i] = rospy.Publisher(
'/xtdrone/' + self.multirotor_type + '_' + str(i) +
'/cmd_vel_flu',
Twist,
queue_size=10)
self.multi_cmd_pub[i] = rospy.Publisher(
'/xtdrone/' + self.multirotor_type + '_' + str(i) + '/cmd',
String,
queue_size=10)
self.leader_cmd_vel_flu_pub = rospy.Publisher(
"/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd",
String,
queue_size=10)
else:
self.multi_cmd_accel_flu_pub = [None] * self.multirotor_num
self.multi_cmd_pub = [None] * self.multirotor_num
for i in range(self.multirotor_num):
self.multi_cmd_accel_flu_pub[i] = rospy.Publisher(
'/xtdrone/' + self.multirotor_type + '_' + str(i) +
'/cmd_accel_flu',
Twist,
queue_size=10)
self.multi_cmd_pub[i] = rospy.Publisher(
'/xtdrone/' + self.multirotor_type + '_' + str(i) + '/cmd',
String,
queue_size=10)
self.leader_cmd_accel_flu_pub = rospy.Publisher(
"/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd",
String,
queue_size=10)
last_forward = 0.0
last_upward = 0.0
last_leftward = 0.0
last_orientation = 0.0
last_ctrl_leader = False
last_cmd_vel_mask = False
num = 0
rate = rospy.Rate(30)
check_stop_flag = False
print('StartRun!')
while True:
num += 1
#print(check_stop_flag)
if self.q_forward.empty():
self.twist.linear.x = last_forward
else:
self.twist.linear.x = self.q_forward.get()
last_forward = self.twist.linear.x
if self.q_upward.empty():
self.twist.linear.z = last_upward
else:
self.twist.linear.z = self.q_upward.get()
last_upward = self.twist.linear.z
if self.q_leftward.empty():
self.twist.linear.y = last_leftward
else:
self.twist.linear.y = self.q_leftward.get()
last_leftward = self.twist.linear.y
if self.q_orientation.empty():
self.twist.angular.z = last_orientation
else:
self.twist.angular.z = self.q_orientation.get()
last_orientation = self.twist.angular.z
if self.q_cmd.empty():
self.cmd = ''
else:
self.cmd = self.q_cmd.get()
if self.q_ctrl_leader.empty():
self.ctrl_leader = last_ctrl_leader
else:
self.ctrl_leader = self.q_ctrl_leader.get()
last_ctrl_leader = self.ctrl_leader
if self.q_cmd_vel_mask.empty():
self.cmd_vel_mask = last_cmd_vel_mask
else:
self.cmd_vel_mask = self.q_cmd_vel_mask.get()
last_cmd_vel_mask = self.cmd_vel_mask
if self.q_stop_flag.empty():
pass
else:
check_stop_flag = self.q_stop_flag.get()
if check_stop_flag:
self.cmd = 'AUTO.RTL'
for i in range(self.multirotor_num):
if self.ctrl_leader:
if self.control_type == 'vel':
self.leader_cmd_vel_flu_pub.publish(self.twist)
else:
self.leader_cmd_accel_flu_pub.publish(self.twist)
self.leader_cmd_pub.publish(self.cmd)
else:
if not self.cmd_vel_mask:
if self.control_type == 'vel':
self.multi_cmd_vel_flu_pub[i].publish(self.twist)
else:
self.multi_cmd_accel_flu_pub[i].publish(self.twist)
self.multi_cmd_pub[i].publish(self.cmd)
rate.sleep()
if check_stop_flag:
self.q_stop_flag.put(True)
rospy.signal_shutdown('STOP!')
break
def display(self, data):
self.text_show_info.setPlainText(data)
def plot(self, data):
for i in range(self.multirotor_num):
self.m.ax.plot(data[i][0], data[i][1], color=self.color_plot[i])
# self.m.canvas_update(self.map)
self.m.draw()