def Zoom():
global limits,frame,zoom,roi,rows,cols,x,y
pygame.event.get()
if joystick.get_button(4) == 1: zoom -= 0.25
elif joystick.get_button(5) == 1: zoom += 0.25
if zoom < 1: zoom = 1
hat = joystick.get_hat(0)
x = x + 10*hat[0]
y = y - 10*hat[1]
if x < 50: x = 50
elif x > cols-50: x = cols-50
if y < 50: y = 50
elif y > rows-50: y = rows-50
limits[0] = y-int(rows/zoom)
limits[1] = y+int(rows/zoom)
limits[2] = x-int(cols/zoom)
limits[3] = x+int(cols/zoom)
if limits[0]<0: limits[0] = 0
if limits[1] > rows: limits[1] = rows
if limits[2]<0: limits[2] = 0
if limits[3] > cols: limits[3] = cols
roi = frame[limits[0]+2:limits[1]-2,limits[2]+2:limits[3]-2]
cv2.rectangle(frame,(limits[2],limits[0]),(limits[3],limits[1]),(0,0,255),2)
frame_Zoom = cv2.resize(frame,(320,240))
ui.Pressure_LCD_2.display(zoom)
ui.Frame_Zoom.setPixmap(QPixmap(QtGui.QImage(frame_Zoom,frame_Zoom.shape[1],frame_Zoom.shape[0],frame_Zoom.strides[0],QtGui.QImage.Format_RGB888)))
def Heading():
global Hea_Backgrnd,Hea_Backgrnd_copy,Yaw_Phone
M = cv2.getRotationMatrix2D((210/2,210/2),Yaw_Phone,1)
Mark = cv2.warpAffine(Wheel,M,(210,210))
for c in range(0,3):
Hea_Backgrnd[0:210,0:210, c] = Mark[:,:,c] * (Mark[:,:,3]/255.0) + Hea_Backgrnd[0:210, 0:210, c] * (1.0 - Mark[:,:,3]/255.0)
ui.Heading_Image.setPixmap(QPixmap(QtGui.QImage(Hea_Backgrnd,Hea_Backgrnd.shape[1],Hea_Backgrnd.shape[0],Hea_Backgrnd.strides[0],QtGui.QImage.Format_RGB888)))
Hea_Backgrnd = Hea_Backgrnd_copy.copy()
def image_callback(self, data):
"""Image callback"""
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
rgb_image = CvBridge().imgmsg_to_cv2(data, desired_encoding="rgb8")
height, width, channel = rgb_image.shape
bytesPerLine = 3 * width
qImg = QtGui.QImage(rgb_image.data, width, height, bytesPerLine, QtGui.QImage.Format_RGB888)
self.videoframe.setPixmap(QtGui.QPixmap(qImg))
def Turn():
pygame.event.get()
global T_Backgrnd,T_Backgrnd_copy,Yaw
Yaw = float("{:>6.3f}".format(joystick.get_axis( 2 ))) - float("{:>6.3f}".format(joystick.get_axis( 5 )))
Yaw = Yaw*(-25)
for c in range(0,3):
T_Backgrnd[19:19+14, 98+Yaw:98+Yaw+14, c] = Ball[:,:,c] * (Ball[:,:,3]/255.0) + T_Backgrnd[19:19+14, 98+Yaw:98+Yaw+14, c] * (1.0 - Ball[:,:,3]/255.0)
ui.Turn_Image.setPixmap(QPixmap(QtGui.QImage(T_Backgrnd,T_Backgrnd.shape[1],T_Backgrnd.shape[0],T_Backgrnd.strides[0],QtGui.QImage.Format_RGB888)))
T_Backgrnd = T_Backgrnd_copy.copy()
cv2.waitKey(1)
def display():
global ui,roi,zoom_flag
temp = roi.shape[0]/(1.0*roi.shape[1])
if temp > 75:
frame_main = cv2.resize(roi,(int(624/temp),624))
elif temp < 75:
frame_main = cv2.resize(roi,(832,int(832*temp)))
else:
frame_main = cv2.resize(roi,(832,624))
ui.Frame.setPixmap(QPixmap(QtGui.QImage(frame_main,frame_main.shape[1],frame_main.shape[0],frame_main.strides[0],QtGui.QImage.Format_RGB888)))
window.show()
cv2.waitKey(1)
def Attitude():
pygame.event.get()
global Needle,H_Backgrnd,Grnd_sky,Pitch,Roll,Sky,flag
Pitch = float("{:>6.3f}".format(joystick.get_axis( 4 )))
Pitch = Pitch * (-40)
Roll = float("{:>6.3f}".format(joystick.get_axis( 3 )))
Roll = Roll * (-20)
Sky = Grnd_sky[Pitch+197:Pitch+197+210,0:210].copy()
for c in range(0,3):
Sky[0:210,0:210, c] = H_Backgrnd[:,:,c] * (H_Backgrnd[:,:,3]/255.0) + Sky[0:0+210, 0:210, c] * (1.0 - H_Backgrnd[:,:,3]/255.0)
M = cv2.getRotationMatrix2D((210/2,210/2),Roll,1)
Mark = cv2.warpAffine(Needle,M,(210,210))
for c in range(0,3):
Sky[0:210,0:210, c] = Mark[:,:,c] * (Mark[:,:,3]/255.0) + Sky[0:210, 0:210, c] * (1.0 - Mark[:,:,3]/255.0)
flag = 1
ui.Attitude_Image.setPixmap(QPixmap(QtGui.QImage(Sky,Sky.shape[1],Sky.shape[0],Sky.strides[0],QtGui.QImage.Format_RGB888)))
def display_frame():
global ui,frame
frame_main = cv2.resize(frame,(832,624))
ui.Frame.setPixmap(QPixmap(QtGui.QImage(frame_main,frame_main.shape[1],frame_main.shape[0],frame_main.strides[0],QtGui.QImage.Format_RGB888)))
window.show()
cv2.waitKey(1)
def api_callback(self, data):
"""API callback from cloud"""
print("Robot in position")
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Move to new waypoint'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Sent'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Move to new waypoint')
self.table_row_tracker += 1
command = 'python post_goal.py %s' % self.num_goal_reached
_id = rospy.get_caller_id()
_class = self.plan[self.num_goal_reached-1]
print(_class)
if len(_class) > 3:
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Move head to position'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Ongoing'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Move head to position')
self.table_row_tracker += 1
time.sleep(10)
self.tableWidget.setItem(self.table_row_tracker-1, 3, QtWidgets.QTableWidgetItem('Complete'))
self.post_goal()
print("New goal sent, waiting for database to update")
time.sleep(8)
URL = 'http://dr0nn1.ddns.net:5000/%s/1' % _class
response = requests.get(URL)
content = response.json()
tag = 'N/A'
value = 'N/A'
warning = 'N/A'
alarm = 'N/A'
value = 'N/A'
if _class == 'manometers':
value = float(content['value'])
warning_low = float(content['low_warning_limit'])
warning_high = float(content['high_warning_limit'])
alarm_low = float(content['low_alarm_limit'])
alarm_high = float(content['high_alarm_limit'])
if value > warning_high and value < alarm_high:
warning = 'Warning high'
elif value < warning_low and value > alarm_low:
warning = 'Warning low'
elif value > alarm_high:
alarm = 'Alarm high'
elif value < alarm_low:
alarm = 'Alarm low'
tag = content['tag']
if _class == 'valve':
normal_condition = content['normal_condition']
should_be = content['is_open']
tag = content['tag']
if value is not should_be:
warning = 'Not in position'
IMG = content['img'].encode('latin1')
rgb_image = np.fromstring(IMG, dtype=np.uint8).reshape((480, 640, 3))
height, width, channel = rgb_image.shape
bytesPerLine = 3 * width
qImg = QtGui.QImage(rgb_image.data, width, height, bytesPerLine, QtGui.QImage.Format_RGB888)
self.column_images[self.column_image_counter].setPixmap(QtGui.QPixmap(qImg))
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem(tag))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Inspecting equipment'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Complete'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem(value))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem(warning))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem(alarm))
self.runningTaskLabel.setText('Inspecting equipment')
self.table_row_tracker += 1
self.column_image_counter += 1
if self.column_image_counter > 2:
self.column_image_counter = 0
else:
time.sleep(2)
self.post_goal()
