btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

暂存

Browse Source 
fix: 修复task多次开启问题
feat: 新增 891 参数（医院停早，红球靠外）
pref: 弃用 action 类
This commit is contained in:
bmy
2024-08-17 01:40:07 +08:00
parent 7ae8162da7
commit 91d6491f64
10 changed files with 1828 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
action.py
View File

@@ -13,15 +13,19 @@ move = None
axis = None
cmd = None
action_run_flag = None
cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
def import_obj(_bycmd):
def import_obj(_run_flag,_bycmd):
global bycmd
global move
global axis
global cmd
global action_run_flag
bycmd = _bycmd
action_run_flag = _run_flag
move = move_cls()
axis = axis_cls()
cmd = cmd_cls()
@@ -118,7 +122,7 @@ class cmd_cls():
else:
time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1:
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
pass
time.sleep(time_via)
def z2(self, _speed, _pos, _time_via = -1):
@@ -131,7 +135,7 @@ class cmd_cls():
else:
time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1:
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
pass
time.sleep(time_via)
def x(self, _speed, _dis, _time_via = -1):
@@ -143,7 +147,7 @@ class cmd_cls():
else:
time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1:
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
pass
time.sleep(time_via)
def x2(self, _speed, _pos, _time_via = -1):
@@ -156,30 +160,30 @@ class cmd_cls():
else:
time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1:
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
pass
time.sleep(time_via)
def camera(self, angle):
while bycmd.send_angle_camera(angle) == -1:
while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet():
pass
def claw(self, angle):
while bycmd.send_angle_claw(angle) == -1:
while bycmd.send_angle_claw(angle) == -1 and action_run_flag.isSet():
pass
time.sleep(0.5)
def claw_arm(self, angle):
while bycmd.send_angle_claw_arm(angle) == -1:
while bycmd.send_angle_claw_arm(angle) == -1 and action_run_flag.isSet():
pass
time.sleep(1)
def scoop(self, angle):
while bycmd.send_angle_scoop(angle) == -1:
while bycmd.send_angle_scoop(angle) == -1 and action_run_flag.isSet():
pass
time.sleep(0.5)
def storage(self, angle):
while bycmd.send_angle_storage(angle) == -1:
while bycmd.send_angle_storage(angle) == -1 and action_run_flag.isSet():
pass
time.sleep(0.5)
def zhuan(self, angle):
while bycmd.send_angle_zhuan(angle) == -1:
while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet():
pass
time.sleep(0.5)
def wait(self, _time):

22
app.py
View File

@@ -23,7 +23,7 @@ processes = []
time_record = None
task_run_flag = False
# 日志队列
queue = Queue()
@@ -37,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!'
socketio = SocketIO(app, allow_unsafe_werkzeug=True)
server_process = None
# FIXME 在列表里存所有的 task_process
task_process = None
task_run_flag = threading.Event()
class WebSocketHandler(logging.Handler):
def emit(self, record):
@@ -124,23 +126,23 @@ def operate_handle(data):
elif data['type'] == 'operate_task':
# 任务函数
if data['content'] == 'run':
task_run_flag = True
task_run_flag.set()
# 开启 task 进程前先关闭所有历史进程
if task_process != None:
task_process.terminate()
time_record = time.perf_counter()
task_process = Process(target=main_func, args=(queue,skip_task_queue))
task_process = Process(target=main_func, args=(task_run_flag,queue,skip_task_queue))
task_process.start()
logger.info("开启 task")
elif data['content'] == 'stop':
task_run_flag = False
task_process.terminate()
task_run_flag.clear()
if task_process != None:
task_process.terminate()
logger.info(f"任务结束 用时{time.perf_counter() - time_record}s")
logger.info("关闭 task")
elif data['content'] == 'restart':
if task_process != None:
task_process.terminate()
task_process = Process(target=main_func, args=(queue,skip_task_queue))
task_process.start()
pass
elif data['type'] == 'show_server_log':
content = ''
try:
@@ -167,7 +169,7 @@ def test_connect():
for item in fileOptions_list:
config_data[item] = toml.load(os.path.join(fileOptions_path,item))
socketio.emit('config_data', {'type': 'config_data', 'content': config_data})
socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag)})
socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
def thread_function():
global queue
while True:

10
cfg_subtask.toml
View File

@@ -1,5 +1,5 @@
[get_block]
pid_kp = 1.0
pid_kp = 1.5
pid_ki = 0
pid_kd = 0
first_block = "blue"
@@ -15,7 +15,7 @@ pid_ki = 0
pid_kd = 0
[up_tower]
pid_kp = 1.0
pid_kp = 1.8
pid_ki = 0
pid_kd = 0
@@ -25,12 +25,12 @@ pid_ki = 0
pid_kd = 0
[put_bball]
pid_kp = 1.5
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[put_hanoi1]
pid_kp = 0.5
pid_kp = 0.7
pid_ki = 0
pid_kd = 0
@@ -53,7 +53,7 @@ pid_kd = 0
llm_enable = true
[kick_ass]
pid_kp = 0.8
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
pos_gap1 = 150

61
cfg_subtask.toml.891.bak Normal file
View File

@@ -0,0 +1,61 @@
[get_block]
pid_kp = 1.5
pid_ki = 0
pid_kd = 0
first_block = "blue"
[put_block]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[get_bball]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[up_tower]
pid_kp = 1.8
pid_ki = 0
pid_kd = 0
[get_rball]
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
[put_bball]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[put_hanoi1]
pid_kp = 0.7
pid_ki = 0
pid_kd = 0
[put_hanoi2]
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
pos_gap = 160
first_target = "mp"
[put_hanoi3]
pid_kp = 1.5
pid_ki = 0
pid_kd = 0
[move_area]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
llm_enable = true
[kick_ass]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
pos_gap1 = 150
pos_gap2 = 80
target_person = 1

35
main_upper.py
View File

@@ -6,12 +6,12 @@ import subtask as sb
import majtask as mj
from by_cmd_py import by_cmd_py
import time
import action as act
# import action as act
import logging
import signal
running = True
def main_func(_queue, _skip_queue):
# running = True
def main_func(_run_flag,_queue, _skip_queue):
if _queue != None:
# 日志重定向
class Handler(logging.Handler):
@@ -23,14 +23,13 @@ def main_func(_queue, _skip_queue):
logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
def signal_handler(sig, frame):
global running
running = False
_run_flag.clear()
signal.signal(signal.SIGTERM, signal_handler)
cmd_py_obj = by_cmd_py(_queue)
sb.import_obj(cmd_py_obj, _skip_queue)
act.import_obj(cmd_py_obj)
# act.import_obj(_run_flag, cmd_py_obj)
# 读取配置
cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
@@ -42,12 +41,20 @@ def main_func(_queue, _skip_queue):
logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
logger.info(cfg_args)
act.axis.camera(0)
act.axis.x2(140)
act.axis.storage(20)
act.axis.scoop(25)
act.axis.claw_arm(225)
act.axis.exec()
cmd_py_obj.send_angle_camera(0)
cmd_py_obj.send_position_axis_x(1, 140)
cmd_py_obj.send_angle_storage(20)
cmd_py_obj.send_angle_scoop(25)
# cmd_py_obj.send_angle_claw_arm(225)
time.sleep(1)
# 弃用 action
# act.axis.camera(0)
# act.axis.x2(140)
# act.axis.storage(20)
# act.axis.scoop(25)
# act.axis.claw_arm(225)
# act.axis.exec()
logger.info(cfg_main)
@@ -72,7 +79,7 @@ def main_func(_queue, _skip_queue):
task_queuem_t = sb.task_queuem(task_queue)
# 创建任务队列的工作线程
def worker_thread():
while task_queuem_t.exec(_skip_queue) is True:
while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
pass
# 启动工作线程
@@ -83,7 +90,7 @@ def main_func(_queue, _skip_queue):
# 创建主任务
main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
try:
while running:
while _run_flag.isSet():
if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
pass
else:

72
subtask.py
View File

@@ -10,7 +10,6 @@ import toml
import zmq
import time
import variable as var
import action as act
import re
import math
import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
ret, error = filter.aim_right(label)
error += offset
# if ret:
if abs(error) <= 8:
if abs(error) <= 10:
car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label)
while not ret:
ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
# ret, box = filter.get(label)
# while not ret:
# ret, box = filter.get(label)
# error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label)
while not ret:
ret, box = filter.get(target_label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
# ret, box = filter.get(target_label)
# while not ret:
# ret, box = filter.get(target_label)
# error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
break
return True
@@ -426,8 +425,7 @@ class task_queuem(task):
class get_block1():
def init(self):
var.task_speed = 15
act.cmd.camera(0)
act.cmd.z2(20, 60, 0)
by_cmd.send_angle_camera(0)
filter.switch_camera(1)
# if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK
@@ -464,7 +462,7 @@ class get_block1():
logger.info("抓取块")
by_cmd.send_position_axis_z(30, 80)
time.sleep(1)
time.sleep(1.5)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
@@ -557,7 +555,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0:
width = box[0][2] - box[0][0]
if width > 130: #135
if width > 125: #135 130 128
return True
return False
def exec(self):
@@ -568,7 +566,7 @@ class put_block():
by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 30) # 20
by_cmd.send_position_axis_x(1, 50) # 20
time.sleep(1)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -593,7 +591,7 @@ class put_block():
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 30)
by_cmd.send_position_axis_x(1, 50)
time.sleep(1.5)
by_cmd.send_angle_claw(45)
time.sleep(1)
@@ -651,6 +649,7 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
logger.info("抓蓝色球")
time.sleep(0.5)
@@ -672,7 +671,7 @@ class get_bball():
time.sleep(0.5)
by_cmd.send_angle_claw_arm(45)
time.sleep(1)
by_cmd.send_position_axis_z(30, 155)
# by_cmd.send_position_axis_z(30, 155)
# 继续向前走
# by_cmd.send_speed_x(4)
pass
@@ -710,10 +709,10 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120)
by_cmd.send_distance_x(-10, 100)
time.sleep(1)
# 上古參數
by_cmd.send_distance_y(-10, 50) # 80
# by_cmd.send_distance_y(-10, 50) # 80
# 6_9 模型參數
# by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數
@@ -724,6 +723,11 @@ class up_tower():
# time.sleep(3)
# by_cmd.send_speed_y(-10)
# time.sleep(0.15)
# 8822
# by_cmd.send_distance_y(-10, 50)
# 891
by_cmd.send_distance_y(-10, 35)
time.sleep(0.3)
by_cmd.send_angle_zhuan(10)
time.sleep(12)
@@ -770,8 +774,8 @@ class get_rball():
by_cmd.send_angle_scoop(20)
# 上古參數
# by_cmd.send_distance_y(-15, 50) # 50 # 70
by_cmd.send_distance_y(-15, 40) # 50 # 70
time.sleep(1.5)
# by_cmd.send_distance_y(-15, 40) # 50 # 70
# time.sleep(1.5)
# 6_9 參數
# by_cmd.send_distance_y(-15, 35)
# time.sleep(2)
@@ -779,6 +783,12 @@ class get_rball():
# by_cmd.send_distance_y(-15, 45)
# time.sleep(2)
# car_stop()
# 8822 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(1.5)
# 891 参数
by_cmd.send_distance_y(-15, 30)
time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1)
logger.info("抓红球")
@@ -826,14 +836,14 @@ class put_bball():
calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
# by_cmd.send_distance_x(10, 10)
# 向左运动
by_cmd.send_distance_y(-10, 35)
# by_cmd.send_distance_y(-10, 35)
# by_cmd.send_angle_storage(10)
# time.sleep(1)
by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里")
time.sleep(1)
time.sleep(2)
# by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20)
# time.sleep(1)
@@ -959,7 +969,7 @@ class put_hanoi1():
def after(self):
# var.switch_lane_model = True
if utils.direction == tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass
@@ -1102,7 +1112,7 @@ class put_hanoi2():
time.sleep(1)
pass
# ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return
@@ -1163,7 +1173,7 @@ class put_hanoi2():
time.sleep(2)
pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return
@@ -1537,14 +1547,6 @@ class move_area2():
if len(resp_commands) == 0:
return
action_list = resp_commands
# 先检查一下 action 是否生成正确，如果不正确直接跳过
actions_keys = self.action_dict.keys()
try:
for action in action_list:
if not (action.get('action') in actions_keys):
return
except:
return
# 进入停车区域
by_cmd.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5)
@@ -1656,6 +1658,8 @@ class kick_ass():
# by_cmd.send_speed_x(25)
# time.sleep(4)
pass
def nexec(self):
pass
def after(self):

2
subtask_8822.py
View File

@@ -557,7 +557,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0:
width = box[0][2] - box[0][0]
if width > 135:
if width > 130:
return True
return False
def exec(self):

1667
subtask_891.py Normal file
View File

File diff suppressed because it is too large Load Diff

5
test/test_filter.py
View File

@@ -25,10 +25,11 @@ filter.switch_camera(1)
# label = tlabel.TPLATFORM
while True:
time.sleep(0.2)
ret, box = filter.get(tlabel.SIGN)
ret, box = filter.get(tlabel.HOSPITAL)
if ret:
width = box[0][2] - box[0][0]
error = (box[0][2] + box[0][0] - 320) / 2
logger.error(error)
logger.error(width)
# if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
# # height = box[0][3] - box[0][1]
# logger.error(111)

6
utils.py
View File

@@ -285,7 +285,11 @@ class label_filter:
else:
target_bool = False
target_box = None
expect_boxes = (results[:, 0] == label.value)
if len(label) == 1:
expect_boxes = (results[:, 0] == label[0].value)
elif len(label) == 2:
expect_boxes = np.logical_or(results[:, 0] == label[0].value, results[:, 0] == label[1].value)
boxes = results[expect_boxes, :]
# 在此处过滤
if len(boxes) != 0: