class MonitorServer(threading.Thread): # 接收udp命令的socket def __init__(self, pubs, galileo_status, galileo_status_lock, host='', user_socket_port=20001, buf_size=1024): super(MonitorServer, self).__init__() self.host = host self.usersocket_port = user_socket_port self.buf_size = buf_size self._stop = threading.Event() self.user_server_socket = socket(AF_INET, SOCK_DGRAM) self.user_server_socket.bind((self.host, self.usersocket_port)) self.parser = ReqParser() self.speed_cmd = Twist() self.global_move_pub = pubs["GLOBAL_MOVE_PUB"] self.cmd_vel_pub = pubs["CMD_VEL_PUB"] self.map_save_pub = pubs["MAPSAVE_PUB"] self.elevator_pub = pubs["ELEVATOR_PUB"] self.tilt_pub = pubs["TILT_PUB"] self.charge_pub = pubs["CHARGE_PUB"] self.charge_pose_pub = pubs["CHARGE_POSE_PUB"] self.galileo_status = galileo_status self.galileo_status_lock = galileo_status_lock self.map_thread = MapService(self.galileo_status, self.galileo_status_lock) self.nav_thread = NavigationService(self.galileo_status, self.galileo_status_lock) self.last_nav_time = rospy.Time(0) self.user_socket_remote = None self.nav_task = None self.nav_guide_task = None self.busy_flag = False rospy.loginfo("service started") def get_galileo_cmds(cmds): self.parse_data([map(lambda x: ord(x), list(cmds.data))]) self.cmd_sub = rospy.Subscriber('/galileo/cmds', GalileoNativeCmds, get_galileo_cmds) def stop(self): self.cmd_sub.unregister() if self.user_server_socket != None: self.user_server_socket.close() if not self.map_thread.stopped(): self.map_thread.stop() self._stop.set() def stopped(self): return self._stop.isSet() def run(self): self.user_server_socket.settimeout(2) # 设置udp 2秒超时 等待 while not self.stopped() and not rospy.is_shutdown(): try: data, self.user_socket_remote = self.user_server_socket.recvfrom( self.buf_size) except timeout: continue if not data: break dataList = [] for c in data: dataList.append(ord(c)) self.parse_data(self.parser.unpack_req(dataList)) # 处理命令数据 self.stop() def parse_data(self, cmds): # 等待退出busy状态,最长等待3分钟 wait_busy_count = 3 * 60 * 100 while self.busy_flag and wait_busy_count > 0: time.sleep(0.01) wait_busy_count -= 1 self.busy_flag = True time_now = rospy.Time.now() for count in range(0, len(cmds)): if len(cmds[count]) > 0: self.CONTROL_FLAG = True global_move_flag = Bool() global_move_flag.data = True self.global_move_pub.publish(global_move_flag) if len(cmds[count]) == 2: global_move_flag = Bool() global_move_flag.data = True temp_scale = 1.0 if abs(self.speed_cmd.linear.x) < 1.0: temp_scale = 1.0 else: temp_scale = abs(self.speed_cmd.linear.x) # 判断是否为关机命令 if cmds[count][0] == 0xaa and cmds[count][1] == 0x44: rospy.loginfo("system poweroff") commands.getstatusoutput( 'sudo shutdown -h now') if cmds[count][0] == ord('f'): rospy.loginfo("forward") self.global_move_pub.publish(global_move_flag) self.speed_cmd.linear.x = MAX_VEL * cmds[count][1] / 100.0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('b'): rospy.loginfo("back") self.global_move_pub.publish(global_move_flag) self.speed_cmd.linear.x = -MAX_VEL * cmds[count][1] / 100.0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('c'): rospy.loginfo("circleleft") self.global_move_pub.publish(global_move_flag) if cmds[count][1] > 1: self.speed_cmd.angular.z = max( 0.4, MAX_THETA * cmds[count][1] / 100.0 / temp_scale) else: self.speed_cmd.angular.z = MAX_THETA * \ cmds[count][1] / 100.0 / temp_scale #self.SPEED_CMD.angular.z = MAX_THETA * cmds[count][1]/100.0/2.8 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('d'): rospy.loginfo("circleright") self.global_move_pub.publish(global_move_flag) if cmds[count][1] > 1: self.speed_cmd.angular.z = min( -0.4, -MAX_THETA * cmds[count][1] / 100.0 / temp_scale) else: self.speed_cmd.angular.z = -MAX_THETA * \ cmds[count][1] / 100.0 / temp_scale #self.SPEED_CMD.angular.z = -MAX_THETA * cmds[count][1]/100.0/2.8 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('s'): rospy.loginfo("stop") self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) elif cmds[count][0] == ord('V'): if cmds[count][1] == 0: rospy.loginfo("开启视觉") rospy.loginfo("关闭导航") if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = None tilt_degree = Int16() tilt_degree.data = 0 self.tilt_pub.publish(tilt_degree) if not self.nav_thread.stopped(): self.nav_thread.stop() self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map3.launch'") if self.map_thread.stopped(): rospy.loginfo("开启视觉2") self.map_thread.update = False self.map_thread.start() elif cmds[count][1] == 1: rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") elif cmds[count][1] == 2: rospy.loginfo("保存地图") mapSaveFlag = Bool() mapSaveFlag.data = True self.map_save_pub.publish(mapSaveFlag) if self.map_thread.scale_orb_thread != None: self.map_thread.scale_orb_thread.save_scale() elif cmds[count][1] == 3: rospy.loginfo("更新地图") if self.map_thread.stopped(): rospy.loginfo("开启视觉2") self.map_thread.update = True self.map_thread.start() elif cmds[count][0] == ord('h'): elePose = UInt32() elePose.data = cmds[count][1] self.elevator_pub.publish(elePose) elif cmds[count][0] == ord('m'): time1_diff = time_now - self.last_nav_time if cmds[count][1] == 0: if time1_diff.to_sec() < 30: continue rospy.loginfo("开启视觉,不巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") self.last_nav_time = time1_diff tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) if self.nav_thread.stopped(): self.nav_thread.setspeed(0) self.nav_thread.start() if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = NavigationTask() if cmds[count][1] == 4: rospy.loginfo("关闭自主巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") if self.nav_task is not None: self.nav_task.shutdown() self.nav_task = None tilt_degree = Int16() tilt_degree.data = 0 self.tilt_pub.publish(tilt_degree) if not self.nav_thread.stopped(): self.nav_thread.stop() if self.nav_task is not None: self.nav_task.shutdown() self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map3.launch'") if cmds[count][1] == 5: rospy.loginfo("开启自动巡检") rospy.loginfo("关闭视觉") if not self.map_thread.stopped(): rospy.loginfo("关闭视觉2") self.map_thread.stop() os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") os.system("pkill -f 'roslaunch nav_test update_map.launch'") tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) if self.nav_thread.stopped(): rospy.logwarn("巡检状态未启动") self.busy_flag = False return if self.nav_task.loop_running_flag: rospy.logwarn("已经开始巡检") self.busy_flag = False return self.nav_task.start_loop() if cmds[count][1] == 6: rospy.loginfo("停止自动巡检") if self.nav_task is not None: self.nav_task.stop_loop() elif cmds[count][0] == ord('g'): if self.nav_task is None: self.busy_flag = False return self.nav_task.set_goal(cmds[count][1]) elif cmds[count][0] == ord('i'): if self.nav_task is None: self.busy_flag = False return if cmds[count][1] == 0: self.nav_task.pause() if cmds[count][1] == 1: self.nav_task.resume() if cmds[count][1] == 2: self.nav_task.cancel_goal() elif cmds[count][0] == ord('j'): if cmds[count][1] == 0: # start charge if self.nav_task is not None: charge_msg = Bool() charge_msg.data = True self.charge_pub.publish(charge_msg) if cmds[count][1] == 1: # stop charge charge_msg = Bool() charge_msg.data = False self.charge_pub.publish(charge_msg) if cmds[count][1] == 2: # save charge position save_msg = Bool() save_msg.data = True self.charge_pose_pub.publish(save_msg) if len(cmds[count]) > 2: # insert new goal if cmds[count][0] == ord("g") and cmds[count][1] == ord("i"): pos_x = struct.unpack("f", bytearray(cmds[count][2:6]))[0] pos_y = struct.unpack("f", bytearray(cmds[count][6:10]))[0] if self.nav_task is not None: rospy.logwarn("插入目标点: " + str(pos_x) + " " + str(pos_y)) self.nav_task.insert_goal(pos_x, pos_y, 0) # reset goals if cmds[count][0] == ord("g") and cmds[count][1] == ord("r"): if self.nav_task is not None: self.nav_task.reset_goals() # set loop and sleep time if cmds[count][0] == ord("m") and cmds[count][1] == 5: if self.nav_thread.stopped(): rospy.logwarn("巡检状态未启动") self.busy_flag = False return if self.nav_task is not None: if not self.nav_task.loop_running_flag: tilt_degree = Int16() tilt_degree.data = -19 self.tilt_pub.publish(tilt_degree) self.nav_task.sleep_time = cmds[count][2] self.nav_task.start_loop() else: self.nav_task.sleep_time = cmds[count][2] self.busy_flag = False def sendto(self, data): try: if self.get_connection_status(): self.user_server_socket.sendto( bytes(data), self.user_socket_remote) except: rospy.logwarn("remote disconnect !") def get_connection_status(self): if self.user_server_socket != None and self.user_socket_remote != None: return True else: return False