From e82c254dc43869af079e7a9889746643359f717f Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Fri, 28 Jun 2024 21:34:49 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=96=B0=E5=A2=9E=E6=96=87=E5=BF=83?= =?UTF-8?q?=E4=B8=80=E8=A8=80=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cfg_subtask.toml | 16 ++-- subtask.py | 224 ++++++++++++++++++++++++++++--------------- test/test_filter.py | 227 +++++++++++++++++++++++++++++++++++++++----- test/test_llm.py | 8 +- utils.py | 5 +- 5 files changed, 368 insertions(+), 112 deletions(-) diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 7748e92..14d658e 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -6,8 +6,8 @@ pid_ki = 0 pid_kd = 0 # *第一个抓取的方块 -# first_block = "red" -first_block = "blue" +first_block = "red" +# first_block = "blue" ################################################ [put_block] @@ -40,7 +40,7 @@ pid_kd = 0 ################################################ [put_bball] # pid 参数值 -pid_kp = 1.2 +pid_kp = 0.6 pid_ki = 0 pid_kd = 0 @@ -60,9 +60,9 @@ pid_kd = 0 # 距离标定值 pos_gap = 160 # 标定值，两个放置位置的标定距离 -pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区 -pos_mp = 1 - +# first_target = "lp" +first_target = "mp" +# first_target = "sp" ################################################ [put_hanoi3] # pid 参数值 @@ -77,7 +77,7 @@ pid_kp = 1.4 pid_ki = 0 pid_kd = 0 -llm_enable = false # 大模型机器人 +llm_enable = true # 大模型机器人 ################################################ [kick_ass] @@ -88,5 +88,5 @@ pid_kd = 0 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap2 = 80 # person 之间的距离 -target_person = 2 # 击打的人 - 最靠近标识牌的为 1 +target_person = 3 # 击打的人 - 最靠近标识牌的为 1 ################################################ diff --git a/subtask.py b/subtask.py index f35f924..240bd20 100644 --- a/subtask.py +++ b/subtask.py @@ -194,7 +194,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): if abs(error) < 8: error = error * 3 else: - error = error + error = error * 2 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -324,7 +324,7 @@ class get_block1(): return False def exec(self): car_stop() - calibrate_new(self.target_label, offset = 15, run = True) + calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5) logger.info("抓取块") # act.axis.wait(0.5) # act.axis.z2(60) @@ -350,7 +350,7 @@ class get_block1(): by_cmd.send_angle_claw(63) by_cmd.send_position_axis_x(1, 20) time.sleep(2) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(25) by_cmd.send_beep(1) time.sleep(0.5) by_cmd.send_beep(0) @@ -399,7 +399,7 @@ class get_block2(): return False def exec(self): car_stop() - calibrate_new(self.target_label, offset = 15, run = True) + calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5) logger.info("抓取块") time.sleep(0.5) by_cmd.send_angle_claw_arm(220) @@ -408,7 +408,7 @@ class get_block2(): time.sleep(1) by_cmd.send_position_axis_x(1, 20) time.sleep(1) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(25) by_cmd.send_beep(1) time.sleep(0.5) by_cmd.send_beep(0) @@ -468,7 +468,7 @@ class put_block(): time.sleep(0.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(25) time.sleep(0.5) by_cmd.send_position_axis_z(30, 140) time.sleep(2) @@ -489,7 +489,7 @@ class put_block(): def after(self): var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"]) # 下一动作预备位置 - while by_cmd.send_position_axis_z(30, 135) == -1: + while by_cmd.send_position_axis_z(30, 130) == -1: pass time.sleep(1) while by_cmd.send_position_axis_x(1, 0) == -1: @@ -536,7 +536,7 @@ class get_bball(): car_stop() time.sleep(0.5) for _ in range(3): - calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 4) + calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5) logger.info("抓蓝色球") time.sleep(0.5) by_cmd.send_angle_claw_arm(36) @@ -545,7 +545,7 @@ class get_bball(): time.sleep(1) by_cmd.send_angle_claw(60) time.sleep(1) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(25) time.sleep(1) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) @@ -570,7 +570,8 @@ class get_bball(): # 下一动作预备位置 by_cmd.send_angle_claw(30) time.sleep(0.5) - by_cmd.send_position_axis_z(30, 0) + while by_cmd.send_position_axis_z(30, 0) == -1: + pass time.sleep(1) # # 任务检查间隔 # time.sleep(1) @@ -651,7 +652,7 @@ class get_rball(): # by_cmd.send_angle_scoop(15) time.sleep(0.5) by_cmd.send_position_axis_z(30, 170) - time.sleep(5) + time.sleep(3.5) by_cmd.send_angle_scoop(7) by_cmd.send_distance_y(15, 70) time.sleep(1) @@ -672,9 +673,10 @@ class put_bball(): def init(self): logger.info("派发物资初始化") filter.switch_camera(1) - by_cmd.send_position_axis_z(30, 0) - while (by_cmd.send_angle_camera(90) == -1): - by_cmd.send_angle_camera(90) + while by_cmd.send_position_axis_z(30, 0) == -1: + pass + while by_cmd.send_angle_camera(90) == -1: + pass def find(self): ret = filter.find(tlabel.BASKET) if ret > 0: @@ -684,9 +686,10 @@ class put_bball(): def exec(self): logger.info("找到篮筐") car_stop() - calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 5.5) + calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 6) by_cmd.send_distance_x(10, 20) - by_cmd.send_distance_y(-10, 55) + # 向左运动 + by_cmd.send_distance_y(-10, 35) time.sleep(1) by_cmd.send_angle_storage(55) @@ -709,6 +712,7 @@ class put_bball(): # 物资盘点 class put_hanoi1(): def init(self): + var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"]) logger.info("物资盘点 1 初始化") filter.switch_camera(2) def find(self): @@ -804,23 +808,16 @@ class put_hanoi1(): class put_hanoi2(): def __init__(self): - self.pos_lp = cfg['put_hanoi2']['pos_lp'] - self.pos_mp = cfg['put_hanoi2']['pos_mp'] - self.pos_sp = 6 - self.pos_lp - self.pos_mp - if self.pos_lp == 1: + if cfg['put_hanoi2']['first_target'] == "lp": self.target_label = tlabel.LPILLER - elif self.pos_mp == 1: + elif cfg['put_hanoi2']['first_target'] == "mp": self.target_label = tlabel.MPILLER - else: + elif cfg['put_hanoi2']['first_target'] == "sp": self.target_label = tlabel.SPILLER - self.distance_lp = self.pos_lp * cfg['put_hanoi2']['pos_gap'] - self.distance_mp = self.pos_mp * cfg['put_hanoi2']['pos_gap'] - self.distance_sp = self.pos_sp * cfg['put_hanoi2']['pos_gap'] - logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]") def init(self): logger.info("物资盘点 2 初始化") if utils.direction == tlabel.RMARK: - self.offset = 25 + self.offset = 22 else: self.offset = 10 def find(self): @@ -846,28 +843,28 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 130) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(90) + by_cmd.send_angle_claw(81) time.sleep(1.5) - by_cmd.send_angle_claw(45) - time.sleep(2) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_angle_claw(40) time.sleep(1) + by_cmd.send_distance_axis_z(30, 20) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) - time.sleep(2) + time.sleep(1) pass else: by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(90) + by_cmd.send_angle_claw(81) time.sleep(1.5) - by_cmd.send_angle_claw(45) - time.sleep(2) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_angle_claw(40) time.sleep(1) + by_cmd.send_distance_axis_z(30, 20) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) - time.sleep(2) + time.sleep(1) pass explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5) time.sleep(1) @@ -877,18 +874,24 @@ class put_hanoi2(): time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) - by_cmd.send_angle_claw(90) - time.sleep(1) + by_cmd.send_angle_claw(81) + time.sleep(0.5) + by_cmd.send_angle_claw(63) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) + time.sleep(1) pass else: by_cmd.send_position_axis_x(1, 40) time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) - by_cmd.send_angle_claw(90) - time.sleep(1) + by_cmd.send_angle_claw(81) + time.sleep(0.5) + by_cmd.send_angle_claw(63) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) + time.sleep(1) explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5) time.sleep(1) @@ -898,30 +901,30 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 130) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(85) - time.sleep(2) - by_cmd.send_angle_claw(40) - time.sleep(2) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_angle_claw(75) time.sleep(1) + by_cmd.send_angle_claw(35) + time.sleep(1) + by_cmd.send_distance_axis_z(30, 20) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) - time.sleep(3) + time.sleep(1) pass else: by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(85) - time.sleep(2) - by_cmd.send_angle_claw(40) - time.sleep(2) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_angle_claw(75) time.sleep(1) + by_cmd.send_angle_claw(35) + time.sleep(1) + by_cmd.send_distance_axis_z(30, 20) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) - time.sleep(3) + time.sleep(1) pass - explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) time.sleep(1) logger.info("放中平台") if utils.direction is tlabel.RMARK: @@ -931,7 +934,7 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(90) + by_cmd.send_angle_claw(65) time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) time.sleep(1) @@ -943,12 +946,12 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(90) + by_cmd.send_angle_claw(65) time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) time.sleep(1) - explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5) + explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5) time.sleep(1) logger.info("抓小平台") if utils.direction is tlabel.RMARK: @@ -956,30 +959,30 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 130) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(85) + by_cmd.send_angle_claw(70) time.sleep(1) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(27) time.sleep(1) by_cmd.send_distance_axis_z(30, 10) - time.sleep(1) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) - time.sleep(2) + time.sleep(1) pass else: by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) - by_cmd.send_angle_claw(85) + by_cmd.send_angle_claw(70) time.sleep(1) - by_cmd.send_angle_claw(30) + by_cmd.send_angle_claw(27) time.sleep(1) by_cmd.send_distance_axis_z(30, 10) - time.sleep(1) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) - time.sleep(2) + time.sleep(1) pass - explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) time.sleep(1) logger.info("放小平台") if utils.direction is tlabel.RMARK: @@ -1031,7 +1034,8 @@ class put_hanoi3(): time.sleep(1) return True def exec(self): - by_cmd.send_position_axis_z(30, 100) + while by_cmd.send_position_axis_z(30, 100) == -1: + pass pass def nexec(self): pass @@ -1054,7 +1058,7 @@ class move_area1(): # 停车 ocr 识别文字 调用大模型 car_stop() time.sleep(2) - var.task_speed = 8 + var.task_speed = 12 pass def nexec(self): pass @@ -1068,6 +1072,9 @@ class move_area2(): def init(self): logger.info("应急避险第二阶段初始化") self.offset = 15 + self.delta_x = 0 + self.delta_y = 0 + self.delta_omage = 0 def find(self): # time.sleep(0.001) ret, box = filter.get(tlabel.SHELTER) @@ -1076,25 +1083,88 @@ class move_area2(): if abs(error) < 20: return 5000 return False + def sub_light(self, delay_time): + by_cmd.send_light(1) + time.sleep(delay_time) + by_cmd.send_light(0) + def sub_beep(self,delay_time): + by_cmd.send_beep(1) + time.sleep(delay_time) + by_cmd.send_beep(0) + def sub_move(self, x, y): + self.delta_x += x + self.delta_y += y + + if x != 0: + delay_time = int(abs(x) * 500) + if x > 0: + by_cmd.send_distance_x(15, delay_time) + else: + by_cmd.send_distance_x(-15, delay_time) + elif y != 0: + delay_time = int(abs(y) * 500) + if y > 0: # 向左 + by_cmd.send_distance_y(-15, delay_time) + else: + by_cmd.send_distance_y(15, delay_time) + time.sleep(delay_time / 400 * 1.5) + pass + def sub_turn(self, angle): + self.delta_omage += angle + delay_time = int(abs(angle) * 400 / 90) + if angle < 0: + # 左转 + by_cmd.send_angle_omega(+55, delay_time) + else: + # 右转 + by_cmd.send_angle_omega(-55, delay_time) + time.sleep(delay_time / 300 * 1.5) def exec(self): var.task_speed = 0 logger.info("开始寻找停车区域") car_stop() calibrate_new(tlabel.SHELTER, offset = 15, run = True) - time.sleep(1) + time.sleep(0.5) # 进入停车区域 - by_cmd.send_distance_y(10, 450) + by_cmd.send_distance_y(15, 300) - time.sleep(3) + # time.sleep(1) - # TODO 调用大模型 然后执行动作 - by_cmd.send_light(1) - time.sleep(2) - by_cmd.send_light(0) + # 调用大模型 然后执行动作 + resp_commands = eval(llm_bot.get_command_json("旋转 90 度，发声 1 秒，照亮 1 秒").replace("'''json","").replace("'''",'')) + for command in resp_commands: + logger.info(command) + if command['func'] == 'move': + self.sub_move(float(command['x']), float(command['y'])) + elif command['func'] == 'light': + self.sub_light(int(command['time'])) + elif command['func'] == 'beep': + self.sub_beep(int(command['time'])) + elif command['func'] == 'turn': + self.sub_turn(int(command['angle'])) + pass + time.sleep(0.5) + time.sleep(1) + # 回到原位 + delay_time = int(abs(self.delta_omage) * 400 / 90) + if self.delta_omage < 0: + # 左转 + by_cmd.send_angle_omega(-55, delay_time) + else: + # 右转 + by_cmd.send_angle_omega(55, delay_time) + time.sleep(delay_time / 300 * 1.5) + if self.delta_y > 0: + # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了 + delay_time = 300 - (self.delta_y * 500) + else: + delay_time = 300 + (abs(self.delta_y) * 500) # 离开停车区域 - by_cmd.send_distance_y(-10, 450) - time.sleep(4) + by_cmd.send_distance_y(-15, delay_time) + time.sleep(delay_time / 400 * 1.5) + # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值 + # by_cmd.send_distance_y(-15, 300) pass def nexec(self): pass diff --git a/test/test_filter.py b/test/test_filter.py index 54545eb..3c2b8ce 100644 --- a/test/test_filter.py +++ b/test/test_filter.py @@ -9,6 +9,10 @@ from loguru import logger from utils import tlabel import zmq import time +from by_cmd_py import by_cmd_py +import time +from utils import CountRecord +by_cmd = by_cmd_py() context = zmq.Context() socket = context.socket(zmq.REQ) @@ -17,35 +21,210 @@ logger.info("subtask yolo client init") filter = label_filter(socket) filter.switch_camera(1) -offset = 12 -while True: - time.sleep(0.2) - # ret1 = filter.get_mult(tlabel.RMARK) - # ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK]) - # if ret: - # # print(error) - # if abs(error) < 40: - # logger.info('yes') - label = tlabel.RBLOCK +def car_stop(): + for _ in range(3): + by_cmd.send_speed_x(0) + time.sleep(0.2) + by_cmd.send_speed_omega(0) +# 蓝色球使用 +def calibrate_right_new(label, offset, run = True, run_speed = 3.5): + not_found_counts = 0 + ret, error = filter.aim_right(label) + while not ret: + not_found_counts += 1 + if not_found_counts >= 20: + not_found_counts = 0 + error = -320 # error > 0 front run + logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找") + break + ret, error = filter.aim_right(label) + + error += offset + if abs(error) > 10 and run: + if error > 0: + by_cmd.send_speed_x(-run_speed) + else: + by_cmd.send_speed_x(run_speed) + pass + # 停的位置已经很接近目标，可以直接使用 distance 校准 + else: + # logger.info("停车时误差就小于") + # error = error * 3 + # if error > 0: + # by_cmd.send_distance_x(-10, int(error)) + # else: + # by_cmd.send_distance_x(10, int(-error)) + return + while True: + ret, error = filter.aim_right(label) + while not ret: + ret, error = filter.aim_right(label) + error += offset + if ret: + if abs(error) <= 8: + car_stop() + logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") + + ret, error = filter.aim_right(label) + while not ret: + ret, error = filter.aim_right(label) + error += offset + logger.info(f"calibrate_right_new:停车后的误差是{error}") + if abs(error) > 8: + logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") + error = error * 3 + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + + break +''' +description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车 +如果停车后 error > 8 则使用 distance 校准 +这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准 +param {*} label +param {*} offset +param {*} run +param {*} run_speed +return {*} +''' +def calibrate_new(label, offset, run = True, run_speed = 3.5): + not_found_counts = 0 ret, box = filter.get(label) while not ret: - # 注意这里一定要保证摄像头内有该目标 否则会无限循环 + not_found_counts += 1 + if not_found_counts >= 20: + not_found_counts = 0 + error = -320 # error > 0 front run + logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找") ret, box = filter.get(label) - error = (box[0][2] + box[0][0] - 320) / 2 + offset - # ret, box = filter.get(tlabel.BBLOCK) - if ret: - print(error) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + if abs(error) > 10 and run: + if error > 0: + by_cmd.send_speed_x(-run_speed) + else: + by_cmd.send_speed_x(run_speed) + # 停的位置已经很接近目标，可以直接使用 distance 校准 else: - pass + if abs(error) > 8: + logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准") + # error = error # 3 + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + return + logger.info(f"calibrate_new:停车后误差{error}小于 8 不校准") + return + while True: + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + + error = (box[0][2] + box[0][0] - 320) / 2 + offset + if ret: + if abs(error) <= 10: # 5 + car_stop() + logger.info("calibrate_new:行进时 误差小于 10 直接停车") + 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"calibrate_new:停车后的误差是{error}") + if abs(error) > 8: + logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准") + error = error * 3 + logger.error(f"error * 3{error}") + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + + break +def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): + # run_direc == 1 向前 + if run_direc == 1: + by_cmd.send_speed_x(run_speed) + else: + by_cmd.send_speed_x(-run_speed) + + while True: + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + if ret: + logger.info(f"当前误差:{error}, box[{box[0][2]},{box[0][0]}]") + # 校准速度越大 停车的条件越宽泛 + if abs(error) <= 20: + car_stop() + logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车") + + error_sum = 0 + for _ in range(3): + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + error_sum += error + error_sum /= 3 + # logger.info(f"停车后像素误差:{error}") + logger.info(f"停车后像素误差:{error_sum}") + # if abs(error) > 8: + logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") + error_sum = error_sum * 3 + logger.error(f"error * 3 {error}") + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + + break + +offset = 10 +# by_cmd.send_angle_claw_arm(217) +# while True: +# pass +# while (by_cmd.send_angle_camera(180) == -1): +# by_cmd.send_angle_camera(180) +while (by_cmd.send_angle_camera(180) == -1): + by_cmd.send_angle_camera(180) +# by_cmd.send_speed_x(15) + +find_counts = 0 +label = tlabel.HOSPITAL +record = CountRecord(5) +while True: + time.sleep(0.2) + + + # ret = filter.find(label) + # ret = filter.find(label) + # if ret > 0: + # find_counts += 1 + # if record(label): + # car_stop() + # if find_counts >= 5: + # car_stop() + ret, box = filter.get(label) + while not ret: + ret, box = filter.get(label) + width = box[0][2] - box[0][0] + logger.info(width) + # error = (box[0][2] + box[0][0] - 320) / 2 + offset + # explore_calibrate_new(tlabel.LPILLER, offset = 10, run_direc = 1, run_speed = 5) + # calibrate_new(label, offset = 15, run = True) + # car_stop() + # time.sleep(0.5) + # for _ in range(3): + # calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4) + # logger.info("抓蓝色球") + # time.sleep(5) + # logger.info("抓取块") + # time.sleep(0.1) - # if ret1: - # print(error1) - # if abs(error1) < 30: - # print("you") - # elif ret2: - # print(error2) - # if abs(error2) < 30: - # print("zuo") diff --git a/test/test_llm.py b/test/test_llm.py index 40becf5..6247ee4 100644 --- a/test/test_llm.py +++ b/test/test_llm.py @@ -14,7 +14,7 @@ class LLM: self.model = 'ernie-3.5' self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。 - 严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！''' + 严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据''' self.prompt += '''正确的示例如下： 向左移 0.1m, 向左转弯 85 度 [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}], 向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}], @@ -22,6 +22,8 @@ class LLM: 原地左转 38 度 [{'func': 'turn','angle': -38}], 蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}] 发光或者照亮 5 秒 [{'func': 'light', 'time': 5}] + 向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}], + 向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}], ''' self.messages = [] self.resp = None @@ -47,6 +49,8 @@ if __name__ == "__main__": lmm_bot = LLM() while True: chat = input("输入：") - print(lmm_bot.get_command_json(chat)) + resp = lmm_bot.get_command_json(chat).replace(' ', '').replace('\n', '').replace('\t', '') + + print(eval(resp[7:-3])) diff --git a/utils.py b/utils.py index 43e7002..bf6e2fc 100644 --- a/utils.py +++ b/utils.py @@ -256,6 +256,8 @@ class LLM: 原地左转 38 度 [{'func': 'turn','angle': -38}], 蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}] 发光或者照亮 5 秒 [{'func': 'light', 'time': 5}] + 向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}], + 向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}], ''' self.prompt += '''你无需回复我''' self.messages = [] @@ -277,7 +279,8 @@ class LLM: messages=self.messages, ) self.messages.append(self.resp.to_message()) - return self.resp.get_result() + resp = self.resp.get_result().replace(' ', '').replace('\n', '').replace('\t', '') + return resp[7:-3] class CountRecord: def __init__(self, stop_count=2) -> None: