def selfcheck(): try: # 1.传感器信息 ser_r = setting.ser_r ser_h = setting.ser_h ser_p = setting.ser_p ser_f = setting.ser_f cgq_ser_err = [] if not ser_r: cgq_ser_err.append("001") if not ser_h: cgq_ser_err.append("002") if not ser_p: cgq_ser_err.append("003") if not ser_f: cgq_ser_err.append("004") # 2.摄像头开启状态 camera_status = setting.camera_status # 3.电池电量、电压、温度 robot_charge = setting.robotcharge # 4.运行时间 starttime = setting.run_time starttime = datetime.now() #赋值不就覆盖了? time.sleep(0.1) nowtime = datetime.now() time_seconds = (nowtime - starttime).seconds # 5.里程 distance = setting.distance # 6.分布式传感器 ds_sor_errs = [] rtn = {"message_type": "self_check", "status": {"robotname":setting.robotname,"cgq_ser_err": cgq_ser_err,"camera_status": camera_status, "robot_charge": robot_charge, "runtime": time_seconds, "distance": distance, "ds_sors": ds_sor_errs}} return rtn except: debug.write_debug(debug.LINE(), "selfcheck-error---", traceback.print_exc())
def robotclient(): try: while get_ping_result(): time.sleep(5) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((setting.appserverip, 8082)) lastrtn = {} RecvTread(s).start() # Server_Socket().start() i = 0 while True: rtn = selfcheck() if i == 0 or rtn["status"]["cgq_ser_err"] != lastrtn["status"]["cgq_ser_err"] or \ rtn["status"]["camera_status"] != lastrtn["status"]["camera_status"]: json_packed = utils.pack2bytes(rtn) print([123456, json_packed]) s.send(json_packed) lastrtn = rtn if setting.find_fire_flag: # 火警报警 rtn1 = {"message_type": "find_fire", "status": {"robotname":setting.robotname}} json_packed = utils.pack2bytes(rtn1) print(json_packed) s.send(json_packed) if setting.fault_warning_flag: # if 1 == 10: # 运行故障报警 rtn1 = {"message_type": "fault_warning", "status": {"robotname":setting.robotname}} json_packed = utils.pack2bytes(rtn1) print(json_packed) s.send(json_packed) if setting.running_warning_flag: # if 1 == 1: # 巡视异常报警 rtn1 = {"message_type": "running_warning", "status": {"robotname":setting.robotname}} json_packed = utils.pack2bytes(rtn1) print(json_packed) s.send(json_packed) i = i + 1 if i == 10: i = 0 print(time.strftime("%Y-%m-%d %X")) time.sleep(10) s.close() except: Server_Socket().stop() s.close() print("错误的结束") debug.write_debug(debug.LINE(), "selfcheck-error---", traceback.print_exc())
def get_right_angle_info(ser_r): try: print(["开始右眼计算角度"]) # ser_r.write("L".encode("gbk")) # 关闭传感器数据 # ser_r.reset_input_buffer() ser_r.write("S".encode("gbk")) # 启动右侧传感器 print("右侧传感器打开") while True: a = datetime.now() while True: b = datetime.now() if (b - a).seconds > 5: setting.right_eye_angle = None break str_code = ser_r.read(1).decode("gb18030", "ignore") if str_code == "A": str_code_one = ser_r.read(1).decode("gb18030", "ignore") # print("R or L:", str_code_one) if str_code_one == 'R' or str_code_one == 'L': USART_flag = True str_code_plus = "" while USART_flag: c = datetime.now() if (c - a).seconds > 5: setting.right_eye_angle = None break str_code_two = str( ser_r.read(1).decode("gb18030", "ignore")) if str_code_two != 'D': str_code_plus += str_code_two else: str_code_eye_angle = str_code_plus[:-1] # ser_r.close() setting.right_eye_angle = get_eye_angle( str_code_eye_angle) + 15 break if setting.right_eye_angle: # print("右眼角度:", setting.right_eye_angle) break ser_r.write("M".encode("gbk")) # time.sleep(0.5) if setting.finish_fire: ser_r.write("X".encode("gbk")) ser_r.close() break except: print(["获取右眼角度失败--\n", traceback.print_exc()]) debug.write_debug(debug.LINE(), "main---", traceback.print_exc())
def get_left_angle_info(ser_l): try: print(["开始计算左眼角度"]) ser_l.write("S".encode("gbk")) # 启动左侧传感器 print("左侧传感器打开") while True: ser_l.write("M".encode("gbk")) a = datetime.now() while True: b = datetime.now() if (b - a).seconds > 5: setting.left_eye_angle = None break str_code = ser_l.read(1).decode("gb18030", "ignore") if str_code == "A": str_code_one = ser_l.read(1).decode("gb18030", "ignore") # print("R or L:", str_code_one) if str_code_one == 'R' or str_code_one == 'L': USART_flag = True str_code_plus = "" while USART_flag: c = datetime.now() if (c - a).seconds > 5: setting.left_eye_angle = None break str_code_two = str( ser_l.read(1).decode("gb18030", "ignore")) if str_code_two != 'D': str_code_plus += str_code_two else: str_code_eye_angle = str_code_plus[:-1] # ser_l.close() setting.left_eye_angle = get_eye_angle( str_code_eye_angle) + 20 break if setting.left_eye_angle: print("左眼角度:", setting.left_eye_angle) break # time.sleep(0.5) if setting.finish_fire: ser_l.write("X".encode("gbk")) ser_l.close() break except: print(["获取左眼角度失败--\n", traceback.print_exc()]) debug.write_debug(debug.LINE(), "main---", traceback.print_exc())
def get_planwork_command(): try: move_flag = False command_goal = {} if len(setting.poilist) > 0: if setting.goal_position == []: setting.goal_position = setting.poilist.pop(0) move_flag = True else: if utils.judge_arrive_flag(setting.goal_position[0], setting.goal_position[1], setting.robotPosx1, setting.robotPosy1, 0.1): utils.print2txt("到达目标附件!") utils.print2txt( ["当前机器人位置:--", setting.robotPosx1, setting.robotPosy1]) print("到达目标附件!") print( ["当前机器人位置:--", setting.robotPosx1, setting.robotPosy1]) print([ "目标位置:--", setting.goal_position[0], setting.goal_position[1] ]) setting.goal_position = setting.poilist.pop(0) #这是个啥操作 move_flag = True #是不是需要这样 else: utils.print2txt("未到达目标附近!") utils.print2txt( ["当前机器人位置:--", setting.robotPosx1, setting.robotPosy1]) print("未到达目标附近!") print( ["当前机器人位置:--", setting.robotPosx1, setting.robotPosy1]) print([ "目标位置:--", setting.goal_position[0], setting.goal_position[1] ]) if move_flag: command_goal = setting.command_goal command_goal["x"] = setting.goal_position[0] command_goal["y"] = setting.goal_position[1] command_goal["yaw"] = setting.goal_position[2] return move_flag, command_goal except: debug.write_debug(debug.LINE(), "main---", traceback.print_exc())
def main(): setting.run_time = datetime.now() # utils.get_ping_result() # 工控机跟机器人连接判断能否通信,能通信返回0,跳出循环,执行下一步操作; # 不能通信,返回1,等待2秒后,再次测试能否通信 while utils.get_ping_result(): time.sleep(2) # 每次启动的时候,都要把之前存在的地图删除掉,以防止地图没更新,之后跟机器人通信后,再生成最新的地图 if os.path.exists(setting.mapfile): os.remove(setting.mapfile) # 打开右传感器 ser_r = utils.serial_open(setting.right_eye_port, bps=9600) if ser_r == "error": sys.exit(errorflag.flag["001"]) else: print("右传感器串口打开成功!") setting.ser_r = True # 打开头部水平旋转传感器 ser_h = utils.serial_open(setting.deluge_gun_port, bps=9600) if ser_h == "error": sys.exit(errorflag.flag["002"]) else: print("头部水平传感器串口打开成功!") setting.ser_h = True ''' # 打开水炮垂直高度传感器 ser_p = utils.serial_open(setting.shuipao_width_port) if ser_p == "error": sys.exit(errorflag.flag["003"]) else: print("水炮垂直高度传感器串口打开成功!") setting.ser_p = True # 打开分布式传感器 ser_f = utils.serial_open(setting.fenbushi_port,bps=9600) if ser_f == "error": sys.exit(errorflag.flag["004"]) else: print("分布式传感器串口打开成功!") setting.ser_f = True ''' # 打开红外传感器 ser_l = utils.serial_open(setting.left_eye_port, bps=9600) if ser_l == "error": sys.exit(errorflag.flag["005"]) else: print("红外传感器串口打开成功!") setting.ser_l = True # 工控机建立客户端,对应机器人的服务端,能够从机器人服务端接收命令、从工控机客户端发送命令到机器人服务端 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((setting.server_ip, 6789)) try: # 发送注册命令到机器人服务端,让机器人知道有客户端连接 json_packed = utils.pack2bytes(setting.reg_client_message) s.send(json_packed) # 发送获取机器人所有信息的命令到机器人服务端 json_packed = utils.pack2bytes(setting.get_all_robot_info_message) s.send(json_packed) # 发送获取地图的命令到机器人服务端 json_packed = utils.pack2bytes(setting.get_map_file) s.send(json_packed) # 发送设置信息过滤的命令到机器人服务端,机器人服务端一直在定时的发送很多命令,一些不需要的命令,可以过滤掉 # json_packed = utils.pack2bytes(setting.set_filter) # 设置信息过滤 # s.send(json_packed) # 发送重定位的命令到机器人服务端,机器人在重启的时候可能定位不准确 间隔5秒 json_packed = utils.pack2bytes(setting.reset_message) s.send(json_packed) time.sleep(5) # 启动接收从机器人服务端发送过来的线程,这个线程一直在运行,只要有消息发送过来,就会执行 RecvTread(s).start() # 启动检测火源的线程,从左右传感器接收消息及发送消息到右传感器 # 发送火源后,不能在主线程再使用左右传感器的端口发送消息或者接受消息,否则报错"句柄无效" # 分布式传感器接收火源信息 # 头部水平旋转线程 # 水炮垂直摆动线程 SerleftRecvTread(ser_l).start() SerrightRecvTread(ser_r).start() # SerfenbushiRecvTread(ser_f).start() SerHeadRecvTread(ser_h).start() # SerSPwidthRecvTread(ser_p).start() # 判断小车运行状态,是否出错 # JudgeRobotStatusTread().start() # 启动APP通信模块 # robotclient.robotclient() # 开启主线程循环,执行灭火,充电,计划任务等操作 # 每0.3秒,执行一次查询火源信息,发现火源后,先执行灭火动作,充电和计划任务操作不执行 # 每1秒,判断是否有充电标志,如果有并且没有发现火源,就去充电,不执行计划任务 # 每1秒,判断是否有计划任务,如果有计划任务并且没有发现火源,电量充足,就去执行计划任务 i = 0 fenbushi_num1 = '' while True: # print([time.strftime("%Y-%m-%d %X"), "开始循环111"]) if setting.shutdown_flag: break i = i + 1 time.sleep(0.3) if setting.robotPosx1 == 0.00: continue # 小车上的传感器是否发现火源 if not setting.find_fire_flag: # print([time.strftime("%Y-%m-%d %X"), "小车没发现火源"]) if setting.fen_find_fire_flag: # 判断当前的分布式传感器的序号是否跟上一次一样,如果一样就不用变换路径 if fenbushi_num1 != setting.fenbushi_num: # 发现火源后,取消导航 json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(1) fenbushi_num1 = setting.fenbushi_num command_goal1 = setting.command_goal command_goal1["x"] = setting.robotPosx12 command_goal1["y"] = setting.robotPosy12 json_packed = utils.pack2bytes(command_goal1) s.send(json_packed) print(["前往分布式传感器,Num:%s" % setting.fenbushi_num]) continue # 判断小车到达分布式传感器附近后,小车上的传感器未发现火源,是否要做圆周运动 continue else: print([time.strftime("%Y-%m-%d %X"), "小车发现火源"]) if not setting.finish_fire: find_fire_pos_flag = False if setting.fen_find_fire_flag: time.sleep(3) else: time.sleep(1) # 发现火源后,取消导航 json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(4) # 启动远红外摄像头线程 GetYuanhongwaiangleTread().start() # 1、右传感器是否发现火源,如果传回角度,小车停下,准备调整水炮角度喷水 if setting.right_eye_angle: print([time.strftime("%Y-%m-%d %X"), "右传感器发现火源"]) pass else: # 2、如果右传感器没有发现火源,让远红外摄像头旋转探测,先按照默认水平角度旋转,再将摄像头高度提高逆转回来,旋转过程中, # 发现火源,就让小车,顺着火源的方向前进,直到右传感器发现火源,小车停下 print([ time.strftime("%Y-%m-%d %X"), "右传感器没有发现发现火源,旋转头部远红外摄像头寻找火源" ]) setting.yuanhongwai_need_find_angle = True setting.xuanzhuan_flag = setting.xuanzhuan_flag + 1 # if xuanzhuan_flag == 1: # setting.nishizhenshizhen_xuanzhuan_flag = True setting.head_flag = 0 # setting.head_flag_angle_1 = True yuanhongwai_start_time = datetime.now() now_flag = 0 while True: yuanhongwai_now_time = datetime.now() if setting.yuanhongwai_levelAngle1: if setting.yuanhongwai_levelAngle1 and ( yuanhongwai_now_time - setting.yuanhongwai_now_time1 ).seconds < 1: now_flag = 1 break else: time.sleep(0.2) else: if (yuanhongwai_now_time - yuanhongwai_start_time).seconds > 100: break else: time.sleep(0.2) if now_flag: print([ time.strftime("%Y-%m-%d %X"), setting.robotPosyaw1, setting.yuanhongwai_levelAngle1, "远红外摄像头发现火源" ]) fire_danjiaodu_angle = setting.robotPosyaw1 + setting.yuanhongwai_levelAngle1 setting.yuanhongwai_need_find_angle = False time.sleep(0.5) setting.yuanhongwai_levelAngle = None setting.yuanhongwai_levelAngle1 = None # setting.head_0_flag = True if fire_danjiaodu_angle > 360: fire_danjiaodu_angle = fire_danjiaodu_angle - 360 elif fire_danjiaodu_angle < 0: fire_danjiaodu_angle = fire_danjiaodu_angle + 360 else: pass print([ time.strftime("%Y-%m-%d %X"), fire_danjiaodu_angle, "计算火源相对小车的角度" ]) one_angle_list = utils.one_angle( [setting.robotPosx1, setting.robotPosy1], [fire_danjiaodu_angle]) for poi1 in one_angle_list: if find_fire_pos_flag: json_packed = utils.pack2bytes( setting.cancel_goal) s.send(json_packed) break index = one_angle_list.index(poi1) + 1 command_goal = setting.command_goal command_goal["x"] = poi1[0] command_goal["y"] = poi1[1] json_packed = utils.pack2bytes(command_goal) s.send(json_packed) print([ "这是单眼角度计算的第%s个点," % index, poi1, ",我已经去了,主人。" ]) while True: # 当右侧传感器发现了角度时,取消导航 if setting.right_eye_angle: print([ time.strftime("%Y-%m-%d %X"), index, "单眼角度,右眼找到了角度" ]) json_packed = utils.pack2bytes( setting.cancel_goal) s.send(json_packed) find_fire_pos_flag = True break arrive_flag = utils.judge_arrive_flag( poi1[0], poi1[1], setting.robotPosx1, setting.robotPosy1, 0.1) if arrive_flag: print([ time.strftime("%Y-%m-%d %X"), "这是单眼角度计算的第%s个点," % index, poi1, ",我已经到达。" ]) break time.sleep(0.1) else: # 3、如果右传感器来回旋转也没有发现火源,就让小车,走圆周运动,直到右传感器发现火源位置,小车停下来 print([ time.strftime("%Y-%m-%d %X"), "都没有发现火源,做圆周运动寻找火源" ]) if 90 < setting.robotPosyaw1 < 270: move_xflag = 1 elif 0 < setting.robotPosyaw1 < 90 or 270 < setting.robotPosyaw1 < 360: move_xflag = -1 else: move_xflag = 0 # 计算出圆周上的几个合适坐标点,然后依次出发到五个点 list_poi = utils.circle_move( [setting.robotPosx1, setting.robotPosy1], move_xflag) find_fire_pos_flag = False for poi in list_poi: if find_fire_pos_flag: break index = list_poi.index(poi) + 1 command_goal1 = setting.command_goal command_goal1["x"] = poi[0] command_goal1["y"] = poi[1] json_packed = utils.pack2bytes(command_goal1) s.send(json_packed) print([ "这是圆周运动计算的第%s个点," % index, poi, ",我已经去了,主人。" ]) while True: time.sleep(0.1) # 在运动过程中若右传感器发现火源,停止导航,当前位置是可以灭火的, # 然后将远红外传感器对准右传感器发现火源的位置 if setting.right_eye_angle: if find_fire_pos_flag: json_packed = utils.pack2bytes( setting.cancel_goal) s.send(json_packed) break else: arrive_flag = utils.judge_arrive_flag( poi[0], poi[1], setting.robotPosx1, setting.robotPosy1, 0.1) if arrive_flag: print([ time.strftime("%Y-%m-%d %X"), "这是圆周运动计算的第%s个点," % index, poi, ",我已经到达。" ]) break # 最后一步,到达灭火目标点,小车停下,根据右传感器传回的角度转动车身,调整远红外摄像头到0度,对准火源,然后根据远红外摄 # 像头返回的火源角度循环调整远红外摄像头,使其能够处在误差范围内的角度。 if setting.right_eye_angle: time.sleep(2) setting.yuanhongwai_need_find_angle = False time.sleep(0.5) setting.head_flag = 2 setting.yuanhongwai_levelAngle = None setting.yuanhongwai_levelAngle1 = None print([time.strftime("%Y-%m-%d %X"), "到达灭火位置"]) print([ time.strftime("%Y-%m-%d %X"), [ setting.robotPosx1, setting.robotPosy1, setting.robotPosyaw1 ], setting.yuanhongwai_levelAngle1, "在灭火地点11111----远红外摄像头发现火源" ]) setting.yuanhongwai_now_time1 = None setting.yuanhongwai_need_find_angle = True time.sleep(1) setting.angle_status = True while True: if setting.judge_angle: print( "--------------------------水平瞄准火源-----------------------" ) break time.sleep(0.02) if setting.judge_angle: time.sleep(1) setting.angle_status_h = True while True: if setting.judge_angle_h: print( "--------------------------竖直瞄准火源-----------------------" ) break time.sleep(0.02) time.sleep(1) if setting.judge_angle_h: setting.shuipao_penshui_flag = True now_time = datetime.now() penshui_status = 0 time.sleep(2) print([ time.strftime("%Y-%m-%d %X"), "灭火时,确定灭火标志00000---", setting.shuipao_penshui_flag ]) while True: penshui_time = datetime.now() if (penshui_time - now_time).seconds > 120: break if setting.yuanhongwai_pitchAngle: penshui_status = 0 else: penshui_status = penshui_status + 1 print([ time.strftime("%Y-%m-%d %X"), "灭火时,未检测到火源---", penshui_status ]) if penshui_status > 15: time.sleep(0.2) setting.shuiyao_stop_penshui_flag = True break setting.yuanhongwai_pitchAngle = None time.sleep(1) print( "--------------------------喷水结束了-----------------------" ) setting.yuanhongwai_levelAngle = None setting.yuanhongwai_levelAngle1 = None setting.finish_fire = True time.sleep(2) command_goal = setting.command_goal command_goal["x"] = 19.688 command_goal["y"] = -19.777 json_packed = utils.pack2bytes(command_goal) s.send(json_packed) break ''' if not setting.finish_fire: # 发现火源后,取消导航 json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(1) if 90 < setting.robotPosyaw1 < 270: move_xflag = 1 elif 0 < setting.robotPosyaw1 < 90 or 270 < setting.robotPosyaw1 < 360: move_xflag = -1 else: move_xflag = 0 # 计算出圆周上的5个合适坐标点,然后依次出发到五个点 list_poi = utils.circle_move([setting.robotPosx1, setting.robotPosy1], move_xflag) find_fire_pos_flag = False for poi in list_poi: if find_fire_pos_flag: break index = list_poi.index(poi) + 1 command_goal1 = setting.command_goal command_goal1["x"] = poi[0] command_goal1["y"] = poi[1] json_packed = utils.pack2bytes(command_goal1) s.send(json_packed) print(["这是圆周运动计算的第%s个点," % index, poi, ",我已经去了,主人。"]) while True: time.sleep(0.1) # 在运动过程中某个传感器发现火源,根据得到的火源方向,计算出此方向上可移动的几个坐标,向前移动, # 直到另一个传感器发现火源停止移动 # if setting.left_eye_angle or setting.right_eye_angle: if setting.left_eye_angle: if find_fire_pos_flag: json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) break json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(2) if setting.left_eye_angle == None: json_packed = utils.pack2bytes(command_goal1) s.send(json_packed) print(["这是圆周运动计算的第%s个点," % index, poi, ",等待8秒后,找不到左眼角度了。"]) else: angle = setting.left_eye_angle print([setting.robotPosx1, setting.robotPosy1],[poi[0], poi[1]]) print([time.strftime("%Y-%m-%d %X"),"单眼角度", angle, setting.robotPosyaw1]) ser_angle = setting.robotPosyaw1 - 90 if ser_angle + angle > 360: ser_angle = ser_angle + angle - 360 else: ser_angle = ser_angle + angle print(["单眼角度寻找点-------", [setting.robotPosx1, setting.robotPosy1],[ser_angle]]) one_angle_list = utils.one_angle([setting.robotPosx1, setting.robotPosy1],[ser_angle]) for poi1 in one_angle_list: if find_fire_pos_flag: json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) break index = one_angle_list.index(poi1) + 1 command_goal = setting.command_goal command_goal["x"] = poi1[0] command_goal["y"] = poi1[1] if setting.left_eye_angle and setting.right_eye_angle: print([time.strftime("%Y-%m-%d %X"), index, "单眼角度,左右眼都找到了角度"]) json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) else: json_packed = utils.pack2bytes(command_goal) s.send(json_packed) print(["这是单眼角度计算的第%s个点," % index, poi1, ",我已经去了,主人。"]) while True: # 当左右两个传感器都发现了角度时,取消导航,测出火源位置坐标, if setting.left_eye_angle and setting.right_eye_angle: print([time.strftime("%Y-%m-%d %X"), index, "单眼角度,左右眼都找到了角度"]) json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(3) if setting.left_eye_angle and setting.right_eye_angle: find_fire_pos_flag = True break else: json_packed = utils.pack2bytes(command_goal) s.send(json_packed) print(["这是单眼角度计算的第%s个点," % index, poi1, ",单眼角度,等待8秒后,左右眼都找不到了角度。"]) arrive_flag = utils.judge_arrive_flag(poi1[0], poi1[1], setting.robotPosx1, setting.robotPosy1, 0.1) if arrive_flag: print([time.strftime("%Y-%m-%d %X"), "这是单眼角度计算的第%s个点," % index, poi1, ",我已经到达。"]) break time.sleep(0.1) elif not setting.left_eye_angle and not setting.right_eye_angle: arrive_flag = utils.judge_arrive_flag(poi[0], poi[1], setting.robotPosx1, setting.robotPosy1, 0.1) if arrive_flag: print([time.strftime("%Y-%m-%d %X"), "这是圆周运动计算的第%s个点," % index, poi, ",我已经到达。"]) break else: pass if setting.left_eye_angle and setting.right_eye_angle: # 当两个传感器都发现火源时,计算火源的位置及头部旋转的角度 rb_pos = [setting.robotPosx1, setting.robotPosy1, setting.robotPosyaw1] fire_pos, deluge_gun_angle = dealfire.get_fire_pos1(setting.left_eye_angle, setting.right_eye_angle, rb_pos) print(["当前机器人位置:--", rb_pos, "水炮旋转角度:--", deluge_gun_angle]) # if deluge_gun_angle - 10 < 0: # deluge_gun_angle = 350 + deluge_gun_angle # else: # deluge_gun_angle = deluge_gun_angle - 10 dealfire.set_deluge_gun_angle(ser_w,deluge_gun_angle) time.sleep(100) # 等待喷水完成,小车移动到某个点 # setting.finish_fire = True command_goal = setting.command_goal command_goal["x"] = setting.robotPosx11 command_goal["y"] = setting.robotPosy11 command_goal["yaw"] = setting.robotPosyaw11 json_packed = utils.pack2bytes(command_goal) s.send(json_packed) while True: time.sleep(0.5) if utils.judge_arrive_flag(setting.robotPosx11, setting.robotPosy11, setting.robotPosx1, setting.robotPosy1, 0.05): utils.print2txt("到达目标附近!") time.sleep(3) break json_packed = utils.pack2bytes(setting.auto_charge_message) s.send(json_packed) ''' # continue if i == 3: i = 0 # 充电模块执行,充电时其他命令不执行 utils.print2txt(['当前的电量:', setting.robotcharge]) print([ time.strftime("%Y-%m-%d %X"), '当前的电量:', setting.robotcharge ]) if setting.charge_status: # 正处在充电状态中 utils.print2txt("正在充电状态中!!!") print("正在充电状态中!!!") if setting.charge_flag: # 充电命令执行一次 json_packed = utils.pack2bytes(setting.cancel_goal) s.send(json_packed) time.sleep(0.5) command_goal = setting.command_goal command_goal["x"] = setting.robotPosx11 command_goal["y"] = setting.robotPosy11 command_goal["yaw"] = setting.robotPosyaw11 json_packed = utils.pack2bytes(command_goal) s.send(json_packed) while True: time.sleep(0.1) if utils.judge_arrive_flag(setting.robotPosx11, setting.robotPosy11, setting.robotPosx1, setting.robotPosy1, 0.05): json_packed = utils.pack2bytes( setting.cancel_goal) s.send(json_packed) utils.print2txt("到达目标附件!") # time.sleep(1) json_packed = utils.pack2bytes( setting.auto_charge_message) s.send(json_packed) break setting.charge_flag = False if setting.stopcharger_flag: # 停止充电执行一次 json_packed = utils.pack2bytes(setting.cancel_charge) s.send(json_packed) setting.charge_status = False setting.stopcharger_flag = False continue # 计划任务模块执行 move_flag, command_goal = planwork.get_planwork_command() if move_flag: print(["执行导航, ", command_goal]) json_packed = utils.pack2bytes(command_goal) s.send(json_packed) s.close() except: print("错误的结束") debug.write_debug(debug.LINE(), "main---", traceback.print_exc()) finally: s.close()