feat: 启用llm任务初版

app.py

@@ -15,9 +15,8 @@ from main_upper import main_func
 server_command = [
     {"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"},
     {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"},
-    # {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"},
     {"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"},
-
+    {"path": "/home/evan/Workplace/project_infer/ocr_server/", "script": "ocr_infer_server.py"},
 ]
 processes = []
 

cfg_args.toml

@@ -10,5 +10,5 @@ UpTower_enable = true
 GetRBall_enable = true
 PutBBall_enable = true
 PutHanoi_enable = true
-MoveArea_enable = false
+MoveArea_enable = true
 KickAss_enable = true

cfg_main.toml

@@ -13,7 +13,7 @@ GetRBall_counts = 10
 PutBBall_counts = 15
 PutHanoi1_counts = 7
 PutHanoi2_counts = 2
-PutHanoi3_counts = 5
+PutHanoi3_counts = 2
 MoveArea1_counts = 6
 MoveArea2_counts = 1700
 KickAss_counts = 10

cfg_subtask.toml

@@ -50,7 +50,7 @@ pid_kd = 0
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
-llm_enable = false
+llm_enable = true
 
 [kick_ass]
 pid_kp = 0.8

subtask.py

@@ -11,8 +11,9 @@ import time
 import variable as var
 import action as act
 import re
-import threading
+import math
-import ctypes
+# import threading
+# import ctypes
 cfg = None
 cfg_args = None
 by_cmd = None
@@ -60,10 +61,10 @@ def import_obj(_by_cmd, skip_queue):
     logger.info("subtask yolo client init")
 
     # ocr socket 客户端
-    # context1 = zmq.Context()
+    context1 = zmq.Context()
-    # ocr_socket = context1.socket(zmq.REQ)
+    ocr_socket = context1.socket(zmq.REQ)
-    # ocr_socket.connect("tcp://localhost:6668")
+    ocr_socket.connect("tcp://localhost:6668")
-    # logger.info("subtask ocr client init")
+    logger.info("subtask ocr client init")
 
     filter = label_filter(socket)
     if cfg['move_area']['llm_enable']:
@@ -1234,6 +1235,7 @@ class move_area1():
         logger.info("应急避险第一阶段初始化")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
+        filter.switch_camera(1)
     def find(self):
         ret = filter.find(tlabel.SIGN)
         if ret:
@@ -1296,6 +1298,31 @@ class move_area1():
 
 # 应急避险 第二阶段 找停车区域
 class move_area2():
+    def __init__(self):
+        self.action_dict = {
+            'beep_seconds': self.beep_seconds,
+            'beep_counts': self.beep_counts,
+            'light_seconds': self.light_seconds,
+            'light_counts': self.light_counts,
+            'beep_light_counts': self.beep_light_counts,
+            'beep_light_seconds': self.beep_light_seconds,
+            'go_front': self.go_front,
+            'go_back': self.go_back,
+            'go_left': self.go_left,
+            'go_right': self.go_right,
+            'go_left_rotate': self.go_left_rotate,
+            'go_right_rotate': self.go_right_rotate,
+            'go_sleep': self.go_sleep
+        }
+        self.front_time = 0
+        self.back_time = 0
+        self.left_time = 0
+        self.right_time = 0
+        self.sum_rotate_angle = 0
+        self.abs_x = 0 # 为了和程序指令适配，其中 x y 方向互换
+        self.abs_y = 0
+        self.abs_w = 0
+        pass
     def init(self):
         logger.info("应急避险第二阶段初始化")
         self.offset = 15
@@ -1303,7 +1330,6 @@ class move_area2():
         self.delta_y = 0
         self.delta_omage = 0
     def find(self):
-        # time.sleep(0.001)
         if var.skip_llm_task_flag:
             return 5000
         ret, box = filter.get(tlabel.SHELTER)
@@ -1312,44 +1338,138 @@ class move_area2():
             if abs(error) < 20:
                 return 5000
         return False
-    def sub_light(self, delay_time):
+    def add_item(self, item):
-        by_cmd.send_light(1)
+        try:
-        time.sleep(delay_time)
+            return self.action_dict[item.get('action')](item.get('time'))
-        by_cmd.send_light(0)
+        except:
-    def sub_beep(self,delay_time):
+            pass
+        return False
+    def beep_seconds(self, _time):
         by_cmd.send_beep(1)
-        time.sleep(delay_time)
+        time.sleep(_time * 0.7)
         by_cmd.send_beep(0)
-    def sub_move(self, x, y):
+        return True
-        # FIXME 如果同時有 xy，是否會造成 delay 不足
+    def beep_counts(self, _time):
-        self.delta_x += x
+        for _ in range(_time):
-        self.delta_y += y
+            by_cmd.send_beep(1)
-        
+            time.sleep(0.3)
-        if x != 0:
+            by_cmd.send_beep(0)
-            delay_time = int(abs(x) * 500)
+            time.sleep(0.2)
-            if x > 0:
+        return True
-                by_cmd.send_distance_x(15, delay_time)
+    def light_seconds(self, _time):
-            else:
+        by_cmd.send_light(1)
-                by_cmd.send_distance_x(-15, delay_time)
+        time.sleep(_time * 0.7)
-        elif y != 0:
+        by_cmd.send_light(0)
-            delay_time = int(abs(y) * 500)
+        return True
-            if y > 0: # 向左
+    def light_counts(self, _time):
-                by_cmd.send_distance_y(-15, delay_time)
+        for _ in range(_time):
-            else:
+            by_cmd.send_light(1)
-                by_cmd.send_distance_y(15, delay_time)
+            time.sleep(0.3)
-        time.sleep(delay_time / 500)
+            by_cmd.send_light(0)
-        car_stop()
+            time.sleep(0.2)
-        pass
+        return True
-    def sub_turn(self, angle):
+    def beep_light_counts(self, _time):
-        self.delta_omage += angle
+        for _ in range(_time):
-        delay_time = int(abs(angle) * 400 / 90)
+            by_cmd.send_beep(1)
-        if angle < 0:
+            by_cmd.send_light(1)
-            # 左转
+            time.sleep(0.3)
-            by_cmd.send_angle_omega(+55, delay_time)
+            by_cmd.send_beep(0)
+            by_cmd.send_light(0)
+            time.sleep(0.2)
+        return True
+    def beep_light_seconds(self, _time):
+        by_cmd.send_beep(1)
+        by_cmd.send_light(1)
+        time.sleep(_time * 0.3)
+        by_cmd.send_beep(0)
+        by_cmd.send_light(0)
+        return True
+    def go_front(self, _time):
+        self.abs_y -=  math.sin(self.abs_w) * _time
+        self.abs_x +=  math.cos(self.abs_w) * _time
+        logger.info(f"向前移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_x(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.front_time += speed_time
+        return True
+    def go_back(self, _time):
+        self.abs_y +=  math.sin(self.abs_w) * _time
+        self.abs_x -=  math.cos(self.abs_w) * _time
+        logger.info(f"向后移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_x(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.back_time += speed_time
+        return True
+    def go_left(self, _time):
+        self.abs_y -=  math.cos(self.abs_w) * _time
+        self.abs_x -=  math.sin(self.abs_w) * _time
+        logger.info(f"向左移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_y(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.left_time += speed_time
+        return True
+    def go_right(self, _time):
+        self.abs_y +=  math.cos(self.abs_w) * _time
+        self.abs_x +=  math.sin(self.abs_w) * _time
+        logger.info(f"向右移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_y(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.right_time += speed_time
+        return True
+    def go_shift(self, _dis_x, _dis_y):
+        direct_x = 1.0 if (_dis_x > 0) else -1.0
+        direct_y = 1.0 if (_dis_y > 0) else -1.0
+        self.abs_y -=  math.sin(self.abs_w) * _dis_x
+        self.abs_x +=  math.cos(self.abs_w) * _dis_x
+        self.abs_y +=  math.cos(self.abs_w) * _dis_y
+        self.abs_x +=  math.sin(self.abs_w) * _dis_y
+        logger.info(f"水平移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time_x = int(abs(_dis_x) * 750)
+        speed_time_y = int(abs(_dis_y) * 750)
+        by_cmd.send_distance_x(10 * direct_x, speed_time_x)
+        by_cmd.send_distance_y(10 * direct_y, speed_time_y)
+        time.sleep(max(speed_time_x, speed_time_y) / 100) #FIXME 除以 100 是否正确
+        return True
+    def go_left_rotate(self, _time):
+        self.abs_w += math.radians(_time) # 弧度制
+        logger.info(f"向左旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        self.sum_rotate_angle -= _time
+        speed_time = int(abs(_time) * 7.25)
+        by_cmd.send_angle_omega(30, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        return True
+    def go_right_rotate(self, _time):
+        self.abs_w -= math.radians(_time) # 弧度制
+        logger.info(f"向右旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        self.sum_rotate_angle += _time
+        speed_time = int(abs(_time) * 7.25)
+        by_cmd.send_angle_omega(-30, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        return True
+    def go_sleep(self, _time):
+        time.sleep(_time*0.7)
+        return True
+    def reset(self):
+        logger.info(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]")
+        # 归一化角度到 0-2pi
+        left_dregee = math.degrees(self.abs_w % (2 * math.pi))
+        # 确定旋转方向 (寻找回正角度最小旋转方向)
+        if math.sin(self.abs_w) < 0:
+            logger.info(f"需要左旋 {360.0 - left_dregee} 回正")
+            self.go_left_rotate(360.0 - left_dregee)
         else:
-            # 右转
+            logger.info(f"需要右旋 {left_dregee} 回正")
-            by_cmd.send_angle_omega(-55, delay_time)
+            self.go_right_rotate(left_dregee)
-        time.sleep(delay_time / 300 * 1.5)
+        time.sleep(0.1)
+
+        # 在框中原点添加向左 0.6m 的偏移值，以便直接回到赛道
+        self.go_shift(self.abs_x * -1.0, self.abs_y * -1.0 - 0.6) 
+
+        logger.info(f"回正后最终位置： ({self.abs_y:.2f}, {self.abs_x:.2f}), 角度： {math.degrees(self.abs_w % (2 * math.pi))}")
     def exec(self):
         var.task_speed = 0
         if var.skip_llm_task_flag:
@@ -1357,8 +1477,9 @@ class move_area2():
             return
         logger.info("开始寻找停车区域")
         car_stop()
-        calibrate_new(tlabel.SHELTER, offset = 15, run = True)
+        # calibrate_new(tlabel.SHELTER, offset = 15, run = True)
         time.sleep(0.5)
+        var.llm_text = "黑暗环境，照亮三秒，左转 90 度，鸣叫三声"
         # 调用大模型 然后执行动作
         try:
             resp = llm_bot.get_command_json(var.llm_text)
@@ -1372,57 +1493,19 @@ class move_area2():
             resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0])
             if len(resp_commands) == 0:
                 return
+            action_list = resp_commands
             # 进入停车区域
-            # by_cmd.send_speed_y(15)
+            by_cmd.send_distance_y(10, 450)
-            by_cmd.send_distance_y(25, 180)
+            time.sleep((450 * 5 / 1000) + 0.5)
-            time.sleep(1)
-            # time.sleep(1.25)
-            car_stop()
-            logger.info(resp_commands)
-            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
-                else:
-                    continue
-                time.sleep(0.5)
         except:
             pass
-        time.sleep(1)
+        for action in action_list:
-        # 回到原位
+            self.add_item(action)
+            time.sleep(0.1)
+            pass
+        time.sleep(0.5)
+        self.reset()
 
-        delay_time = int(abs(self.delta_omage) * 400 / 90)
-        if int(abs(self.delta_omage)) == 360:
-            delay_time = 0
-        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 = 180 - (self.delta_y * 500)
-        else:
-            delay_time = 180 + (abs(self.delta_y) * 500)
-        # 离开停车区域
-        by_cmd.send_distance_y(-25, delay_time)
-
-        time.sleep(delay_time * 5e-3)
-        
-        car_stop()
-
-        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
-        # by_cmd.send_distance_y(-15, 300)
-        pass
     def nexec(self):
         logger.warning("正在跳過大模型任務")
         time.sleep(2)

test/test_action.py

@@ -0,0 +1,241 @@
+import os
+import sys
+import math
+parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.append(parent_dir)
+from by_cmd_py import by_cmd_py
+import time
+import zmq
+import numpy as np
+context = zmq.Context()
+socket = context.socket(zmq.REQ)
+socket.connect("tcp://localhost:6666")
+
+
+def car_stop():
+    for _ in range(3):
+        cmd_py_obj.send_speed_x(0)
+        time.sleep(0.2)
+        cmd_py_obj.send_speed_y(0)
+        time.sleep(0.2)
+        cmd_py_obj.send_speed_omega(0)
+
+class LLM_Action:
+    def __init__(self,cmd_py_obj):
+        self.by_cmd = cmd_py_obj
+        self.action_dict = {
+            'beep_seconds': self.beep_seconds,
+            'beep_counts': self.beep_counts,
+            'light_seconds': self.light_seconds,
+            'light_counts': self.light_counts,
+            'beep_light_counts': self.beep_light_counts,
+            'beep_light_seconds': self.beep_light_seconds,
+            'go_front': self.go_front,
+            'go_back': self.go_back,
+            'go_left': self.go_left,
+            'go_right': self.go_right,
+            'go_left_rotate': self.go_left_rotate,
+            'go_right_rotate': self.go_right_rotate,
+            'go_sleep': self.go_sleep
+        }
+        self.front_time = 0
+        self.back_time = 0
+        self.left_time = 0
+        self.right_time = 0
+        self.sum_rotate_angle = 0
+        self.abs_x = 0 # 为了和程序指令适配，其中 x y 方向互换
+        self.abs_y = 0
+        self.abs_w = 0
+        pass
+    def __call__(self, item):
+        try:
+            return self.action_dict[item.get('action')](item.get('time'))
+        except:
+            pass
+        return False
+    def beep_seconds(self, _time):
+        self.by_cmd.send_beep(1)
+        time.sleep(_time * 0.7)
+        self.by_cmd.send_beep(0)
+        return True
+    def beep_counts(self, _time):
+        for _ in range(_time):
+            self.by_cmd.send_beep(1)
+            time.sleep(0.3)
+            self.by_cmd.send_beep(0)
+            time.sleep(0.2)
+        return True
+    def light_seconds(self, _time):
+        self.by_cmd.send_light(1)
+        time.sleep(_time * 0.7)
+        self.by_cmd.send_light(0)
+        return True
+    def light_counts(self, _time):
+        for _ in range(_time):
+            self.by_cmd.send_light(1)
+            time.sleep(0.3)
+            self.by_cmd.send_light(0)
+            time.sleep(0.2)
+        return True
+    def beep_light_counts(self, _time):
+        for _ in range(_time):
+            self.by_cmd.send_beep(1)
+            self.by_cmd.send_light(1)
+            time.sleep(0.3)
+            self.by_cmd.send_beep(0)
+            self.by_cmd.send_light(0)
+            time.sleep(0.2)
+        return True
+    def beep_light_seconds(self, _time):
+        self.by_cmd.send_beep(1)
+        self.by_cmd.send_light(1)
+        time.sleep(_time * 0.3)
+        self.by_cmd.send_beep(0)
+        self.by_cmd.send_light(0)
+        return True
+    def go_front(self, _time):
+        self.abs_y -=  math.sin(self.abs_w) * _time
+        self.abs_x +=  math.cos(self.abs_w) * _time
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        speed_time = int(abs(_time) * 750)
+        self.by_cmd.send_distance_x(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.front_time += speed_time
+        return True
+    def go_back(self, _time):
+        self.abs_y +=  math.sin(self.abs_w) * _time
+        self.abs_x -=  math.cos(self.abs_w) * _time
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        speed_time = int(abs(_time) * 750)
+        self.by_cmd.send_distance_x(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.back_time += speed_time
+        return True
+    def go_left(self, _time):
+        self.abs_y -=  math.cos(self.abs_w) * _time
+        self.abs_x -=  math.sin(self.abs_w) * _time
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        speed_time = int(abs(_time) * 750)
+        self.by_cmd.send_distance_y(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.left_time += speed_time
+        return True
+    def go_right(self, _time):
+        self.abs_y +=  math.cos(self.abs_w) * _time
+        self.abs_x +=  math.sin(self.abs_w) * _time
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        speed_time = int(abs(_time) * 750)
+        self.by_cmd.send_distance_y(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.right_time += speed_time
+        return True
+    def go_shift(self, _dis_x, _dis_y):
+        direct_x = 1.0 if (_dis_x > 0) else -1.0
+        direct_y = 1.0 if (_dis_y > 0) else -1.0
+        self.abs_y -=  math.sin(self.abs_w) * _dis_x
+        self.abs_x +=  math.cos(self.abs_w) * _dis_x
+        self.abs_y +=  math.cos(self.abs_w) * _dis_y
+        self.abs_x +=  math.sin(self.abs_w) * _dis_y
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        speed_time_x = int(abs(_dis_x) * 750)
+        speed_time_y = int(abs(_dis_y) * 750)
+        self.by_cmd.send_distance_x(10 * direct_x, speed_time_x)
+        self.by_cmd.send_distance_y(10 * direct_y, speed_time_y)
+        time.sleep(max(speed_time_x, speed_time_y) / 100) #FIXME 除以 100 是否正确
+        return True
+    def go_left_rotate(self, _time):
+        self.abs_w += math.radians(_time) # 弧度制
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        self.sum_rotate_angle -= _time
+        speed_time = int(abs(_time) * 7.25)
+        self.by_cmd.send_angle_omega(30, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        # time.sleep(speed_time / _time / 2)
+        return True
+    def go_right_rotate(self, _time):
+        self.abs_w -= math.radians(_time) # 弧度制
+        print(f"abs postion ({self.abs_y:.2f}, {self.abs_x:.2f}) - angle {math.degrees(self.abs_w)} drgee")
+        self.sum_rotate_angle += _time
+        speed_time = int(abs(_time) * 7.25)
+        self.by_cmd.send_angle_omega(-30, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        # time.sleep(speed_time / _time / 2)
+        return True
+    def go_sleep(self, _time):
+        time.sleep(_time*0.7)
+        return True
+    def reset(self):
+        print(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]")
+        # 先复位角度
+        if self.sum_rotate_angle > 0:
+            self.sum_rotate_angle = self.sum_rotate_angle % 360
+        else:
+            self.sum_rotate_angle = -(abs(self.sum_rotate_angle) % 360)
+        # if self.sum_rotate_angle > 0:
+        #     # 采用左转回正
+        #     self.go_left_rotate(self.sum_rotate_angle)
+        #     # speed_time = int(abs(self.sum_rotate_angle) * 7.25)
+        #     # self.by_cmd.send_angle_omega(30, speed_time)
+        #     pass
+        # else:
+        #     # 采用右转回正
+        #     self.go_right_rotate(abs(self.sum_rotate_angle))
+        #     # speed_time = int(abs(self.sum_rotate_angle) * 7.25)
+        #     # self.by_cmd.send_angle_omega(-30, speed_time)
+        left_dregee = math.degrees(self.abs_w % (2 * math.pi)) #归一化角度到 0-2pi
+        if math.sin(self.abs_w) < 0:
+            print(f"需要左旋 {360.0 - left_dregee} 回正")
+            self.go_left_rotate(360.0 - left_dregee)
+        else:
+            print(f"需要右旋 {left_dregee} 回正")
+            self.go_right_rotate(left_dregee)
+        time.sleep(0.1)
+
+        self.go_shift(self.abs_x * -1.0, self.abs_y * -1.0 - 0.6) # 左移 0.6m 回到赛道
+        # # 再回正 x 轴
+        # if self.front_time > self.back_time:
+        #     # 采用后退回正
+        #     speed_time = self.front_time - self.back_time
+        #     self.by_cmd.send_distance_x(-10, speed_time)
+        # else:
+        #     speed_time = self.back_time - self.front_time
+        #     self.by_cmd.send_distance_x(10, speed_time)
+        # time.sleep(speed_time / 100)
+        # time.sleep(0.1)
+          
+        # # 最后回正 y 轴
+        # speed_time = self.left_time - self.right_time
+        # if speed_time < 0:
+        #     speed_time = 4500 + abs(speed_time)
+        # else:
+        #     speed_time = 4500 - speed_time
+        # self.by_cmd.send_distance_y(-10, speed_time / 15 + 100)
+        # print(speed_time * 1e-3 * 0.9)
+        # time.sleep(speed_time * 1e-3 * 0.9)
+
+        print(f"回正后最终位置： ({self.abs_y:.2f}, {self.abs_x:.2f}), 角度： {math.degrees(self.abs_w % (2 * math.pi))}")
+
+
+
+
+
+if __name__ == "__main__":
+
+
+    cmd_py_obj = by_cmd_py()
+    # cmd_py_obj.send_angle_omega(-30, 20 * 7.25)
+    # time.sleep(20 * 7.25 / 20 / 2)
+    llm_act = LLM_Action(cmd_py_obj)
+    action_list = [{"index":0,"action":"go_left_rotate","time":270},{"index":1,"action":"go_back","time":0.2},{"index":2,"action":"go_left","time":0.2},{"index":2,"action":"beep_counts","time":3}]
+    
+    cmd_py_obj.send_distance_y(10, 450)
+    time.sleep((450 * 5 / 1000) + 0.5)
+    # time.sleep(15 * 300 * 1e-3 * 0.7)
+    # car_stop()
+    for action in action_list:
+        llm_act(action)
+        time.sleep(0.1)
+        pass
+    time.sleep(1)
+    llm_act.reset()
+    # car_stop()

test/test_llm.py

@@ -13,18 +13,48 @@ class LLM:
     def __init__(self):
         
         self.model = 'ernie-3.5'
-        self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
+        self.prompt = '''
-                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据'''
+                        你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
-        self.prompt += '''正确的示例如下：
+                        允许的动作及其对应格式如下：
-                        向左移 0.1m, 向左转弯 85 度  [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}],
+                        - 向左移：{"index":N,"action":"go_left","time":T}
-                        向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}],
+                        - 向右移：{"index":N,"action":"go_right","time":T}
-                        向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
+                        - 向前移：{"index":N,"action":"go_front","time":T}
-                        原地左转 38 度 [{'func': 'turn','angle': -38}],
+                        - 向后移：{"index":N,"action":"go_back","time":T}
-                        蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
+                        - 向左转：{"index":N,"action":"go_left_rotate","time":T}
-                        发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+                        - 向右转：{"index":N,"action":"go_right_rotate","time":T}
-                        向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}],
+                        - 蜂鸣器发声：{"index":N,"action":"beep_seconds","time":T}
-                        向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}],
+                        - 蜂鸣器发声次数：{"index":N,"action":"beep_counts","time":T}
+                        - 发光或者照亮：{"index":N,"action":"light_seconds","time":T}
+                        - 发光次数或者闪烁次数：{"index":N,"action":"light_counts","time":T}
+                        - 发光并伴随蜂鸣器：{"index":N,"action":"beep_light_counts","time":T}
+                        - 等待{"index":N,"action":"go_sleep","time":T}
+                        示例输入输出如下：
+                        输入：向左移 0.1m, 向左转弯 85 度
+                        输出：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
+                        输入：向右移 0.2m, 向前 0.1m
+                        输出：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
+                        输入：向右转 90 度，向右移 0.1m
+                        输出：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
+                        输入：原地左转 38 度
+                        输出：[{"index":0,"action":"go_left_rotate","time":38}]
+                        输入：蜂鸣器发声 5 秒
+                        输出：[{"index":0,"action":"beep_seconds","time":5}]
+                        输入：发光或者照亮 5 秒
+                        输出：[{"index":0,"action":"light_seconds","time":5}]
+                        输入：向右走 30cm, 照亮 2s
+                        输出：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
+                        输入：向左移 0.2m, 向后 0.1m
+                        输出：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
+                        输入：鸣叫 3 声
+                        输出：[{"index":0,"action":"beep_counts","time":3}]
+                        输入：前行零点五米
+                        输出：[{"index":0,"action":"go_front","time":0.5}]
+                        输入：闪烁灯光 1 次并伴有蜂鸣器
+                        输出：[{"index":0,"action":"beep_light_counts","time": 1}]
+                        输入：灯光闪烁 3 次同时蜂鸣器也叫 3 次
+                        输出：[{"index":0,"action":"beep_light_counts","time": 3}]
                     '''
+        self.prompt += '''请根据上面的示例，解析该任务文本，并返回相应的 JSON 字段。确保 JSON 中包含了键 index action 和 time 以及相应的值'''
         self.messages = []
         self.resp = None
         self.reset()

utils.py

@@ -381,21 +381,62 @@ class LLM:
         erniebot.ak = "jReawMtWhPu0wrxN9Rp1MzZX"
         erniebot.sk = "eowS1BqsNgD2i0C9xNnHUVOSNuAzVTh6"
         self.model = 'ernie-3.5'
-        self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
+        # self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
-                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
+        #             严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
-        self.prompt += '''正确的示例如下：
+        # self.prompt += '''正确的示例如下：
-                        向左移 0.1m, 向左转弯 85 度  [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}],
+        #                 向左移 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}],
+        #                 向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}],
-                        向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
+        #                 向右转 85 度，向右移 0.1m [{'func': 'turn','angle': 85},{'func': 'move', 'x': 0, 'y': -0.1}],
-                        原地左转 38 度 [{'func': 'turn','angle': -38}],
+        #                 原地左转 38 度 [{'func': 'turn','angle': -38}],
-                        蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
+        #                 蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
-                        发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+        #                 发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
-                        向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}],
+        #                 向右走 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}],
+        #                 向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}],
-                        鸣叫 3 声 [{'func': 'beep', 'time': 3}]
+        #                 鸣叫 3 声 [{'func': 'beep', 'time': 3}]
-                        前行零点五米 [{'func': 'move', 'x': 0.5, 'y': 0}]
+        #                 前行零点五米 [{'func': 'move', 'x': 0.5, 'y': 0}]
+        #             '''
+        self.prompt = '''
+                        你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 JSON 结果。请注意，只能使用以下指定的动作，不能创造新的动作：
+                        允许的动作及其对应格式如下：
+                        - 向左移：{"index":N,"action":"go_left","time":T}
+                        - 向右移：{"index":N,"action":"go_right","time":T}
+                        - 向前移：{"index":N,"action":"go_front","time":T}
+                        - 向后移：{"index":N,"action":"go_back","time":T}
+                        - 向左转：{"index":N,"action":"go_left_rotate","time":T}
+                        - 向右转：{"index":N,"action":"go_right_rotate","time":T}
+                        - 蜂鸣器发声：{"index":N,"action":"beep_seconds","time":T}
+                        - 蜂鸣器发声次数：{"index":N,"action":"beep_counts","time":T}
+                        - 发光或者照亮：{"index":N,"action":"light_seconds","time":T}
+                        - 发光次数或者闪烁次数：{"index":N,"action":"light_counts","time":T}
+                        - 发光并伴随蜂鸣器：{"index":N,"action":"beep_light_counts","time":T}
+                        - 等待{"index":N,"action":"go_sleep","time":T}
+                        示例输入输出如下：
+                        输入：向左移 0.1m, 向左转弯 85 度
+                        输出：[{"index":0,"action":"go_left","time":0.1},{"index":1,"action":"go_left_rotate","time":85}]
+                        输入：向右移 0.2m, 向前 0.1m
+                        输出：[{"index":0,"action":"go_right","time":0.2},{"index":1,"action":"go_front","time":0.1}]
+                        输入：向右转 90 度，向右移 0.1m
+                        输出：[{"index":0,"action":"go_right_rotate","time":90},{"index":1,"action":"go_right","time":0.1}]
+                        输入：原地左转 38 度
+                        输出：[{"index":0,"action":"go_left_rotate","time":38}]
+                        输入：蜂鸣器发声 5 秒
+                        输出：[{"index":0,"action":"beep_seconds","time":5}]
+                        输入：发光或者照亮 5 秒
+                        输出：[{"index":0,"action":"light_seconds","time":5}]
+                        输入：向右走 30cm, 照亮 2s
+                        输出：[{"index":0,"action":"go_right","time":0.3},{"index":1,"action":"light_seconds","time":2}]
+                        输入：向左移 0.2m, 向后 0.1m
+                        输出：[{"index":0,"action":"go_left","time":0.2},{"index":1,"action":"go_back","time":0.1}]
+                        输入：鸣叫 3 声
+                        输出：[{"index":0,"action":"beep_counts","time":3}]
+                        输入：前行零点五米
+                        输出：[{"index":0,"action":"go_front","time":0.5}]
+                        输入：闪烁灯光 1 次并伴有蜂鸣器
+                        输出：[{"index":0,"action":"beep_light_counts","time": 1}]
+                        输入：灯光闪烁 3 次同时蜂鸣器也叫 3 次
+                        输出：[{"index":0,"action":"beep_light_counts","time": 3}]
                     '''
-        self.prompt += '''你只需要根据我的示例解析出指令即可，不要给我其他多余的回复;再次强调 你无需给我其他多余的回复 这对我很重要'''
+        self.prompt += '''请根据上面的示例，解析该任务文本，并返回相应的 JSON 字段。确保 JSON 中包含了键 index action 和 time 以及相应的值'''
         self.messages = []
         self.resp = None
         worker = threading.Thread(target=self.reset, daemon=True)