from SSL_Lib.Camera import Camera
from SSL_Lib.DStar import DStar
from SSL_Lib.utils import *
from SSL_Lib.DBG import DBG
from SSL_Lib.DWA1 import *
import serial
import sys
import time
serialPort = "COM4" # 串口
# 初始化控制和读取的IP地址、端口号
localhost = '127.0.0.1'
control_addr = (localhost, 20011)
read_addr = (localhost, 23333)
camera = Camera(read_addr)
debug = DBG()
# ser=None
# ser = serial.Serial(serialPort, 115200, timeout=0.5)
# ser=config_serial(serialPort)
# 主逻辑
# 1.初始化要控制的机器人
ro_b_0 = Robot('blue', 0, 0.15, control_addr=control_addr)
blue, yellow = camera.getRobotDict() # 读取初始信息
start_point = [blue[0].x, blue[0].y] # 设置机器人开始的位置
end_point = [-start_point[0], -start_point[1]] # 设置机器人终点为对称点
print('start at: ', start_point)
print('goal at: ', end_point)
# 2.目前只测试静态避障，所以只生成一次路径规划
from SSL_Lib.DStar import DStar
from SSL_Lib.utils import *
from SSL_Lib.DBG import DBG
from SSL_Lib.DWA1 import *
import serial
import sys
import time
from SSL_Lib.RRT import *
serialPort = "COM4" # 串口
# 初始化控制和读取的IP地址、端口号
localhost = '127.0.0.1'
control_addr = (localhost, 20011)
read_addr = (localhost, 23333)
camera = Camera(read_addr)
debug = DBG()
# ser=None
# ser = serial.Serial(serialPort, 115200, timeout=0.5)
# ser=config_serial(serialPort)
# 主逻辑
# 1.初始化要控制的机器人
ro_b_0 = Robot('blue', 0, 0.15, control_addr=control_addr)
blue, yellow = camera.getRobotDict() # 读取初始信息
start_point = [blue[0].x, blue[0].y] # 设置机器人开始的位置
end_point = [-start_point[0], -start_point[1]] # 设置机器人终点为对称点
print('start at: ', start_point)
print('goal at: ', end_point)
# 2.目前只测试静态避障，所以只生成一次路径规划
from SSL_Lib.Camera import *
from SSL_Lib.P2P import *
from SSL_Lib.DWA1 import *
from SSL_Lib.DStar import *
from SSL_Lib.DBG import DBG
import matplotlib.pyplot as plt
from SSL_Lib.utils import *
# 初始化控制和读取的IP地址、端口号
show_animation = True
control_addr = ('127.0.0.1', 20011)
read_addr = ('127.0.0.1', 23334)
# 初始化机器人，DEBUG和camera实例
ro_b_0 = Robot("blue", 0, 0.15, control_addr)
debug = DBG()
camera = Camera(read_addr)
# 初始化起点，并自定义终点
blue, yellow = camera.getRobotDict()
x = np.array(
[blue[0].x / 1000, blue[0].y / 1000, blue[0].orientation, 0.0, 0.0])
goal = np.array([-blue[0].x / 1000, -blue[0].y / 1000])
print('start at ', x)
print('goal is ', goal)
# 用来另开一个线程的函数
def getblue0():
global blue
while True:
from SSL_Lib.Camera import Camera
from SSL_Lib.DStar import DStar
from SSL_Lib.utils import *
from SSL_Lib.DBG import DBG
from SSL_Lib.DWA1 import *
import serial
import sys
import time
serialPort = "COM4" # 串口
# 初始化控制和读取的IP地址、端口号
localhost = '127.0.0.1'
control_addr = (localhost, 20011)
read_addr = (localhost, 23333)
camera = Camera(read_addr)
debug = DBG()
# ser=None
# ser = serial.Serial(serialPort, 115200, timeout=0.5)
# ser=config_serial(serialPort)
# 主逻辑
# 1.初始化要控制的机器人
ro_b_0 = Robot('blue', 0, 0.15, control_addr=control_addr)
blue, yellow = camera.getRobotDict() # 读取初始信息(已换算)
start_point = [blue[0].x, blue[0].y] # 设置机器人开始的位置(已换算，单位是m)
end_point = [-start_point[0], -start_point[1]] # 设置机器人终点为对称点(已换算，单位是m)
print('start at: ', start_point)
print('goal at: ', end_point)
# 2.目前只测试静态避障，所以只生成一次路径规划