241 lines
9.2 KiB
Python
241 lines
9.2 KiB
Python
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) * 4.35)
|
|
self.by_cmd.send_angle_omega(50, 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) * 4.35)
|
|
self.by_cmd.send_angle_omega(-50, 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() |