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()