def main():
arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
arduinoSocket.connect((host, port))
# arduinoSocket.setblocking(0)
t = PeriodTimer(0.1)
t.start()
count = 0
num1 = 105
num2 = 50
while True:
try:
with t:
count += 1
num1 += 1
num2 += 1
if num1 > 120:
num1 = 100
if num2 > 130:
num2 = 50
print count
data = [num1, num2]
arduinoSocket.send(dataSend(data))
print data
print dataRead(arduinoSocket)
except (Exception, KeyboardInterrupt):
arduinoSocket.close()
def main():
mio = MIO_TRANS()
t = PeriodTimer(T)
t.start()
try:
while True:
with t:
mio.update()
except (Exception, KeyboardInterrupt):
mio.dev.close()
mio.jsdev.close()
mio.tmdev.close()
if Serial_on:
mio.Arduino_ser.close()
raise
def main():
Arduino_ser = serial.Serial(port='COM3',
baudrate=9600,
bytesize=8,
parity='N',
stopbits=1,
timeout=1)
t = PeriodTimer(0.1)
t.start()
data = 0
while True:
with t:
data += 1
if data > 360:
data = 0
Arduino_ser.write(send_data(data))
print data, data + 1, data + 2, data + 3
read_data(Arduino_ser)
def main():
arduinoSer = serial.serial_for_url(arduinoUrl,
baudrate=115200,
do_not_open=True,
timeout=1)
dev = MsgDevice()
dev.open()
dev.pub_bind(msgPubBind)
dev.sub_connect(msgSubConnect)
dev.sub_add_url('motorSpeed')
dev.sub_add_url('maxPower')
for i in range(7):
dev.sub_add_url('rudder' + str(i) + 'Ang')
t = PeriodTimer(0.1)
t.start()
try:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
arduinoSer.close()
print e, ", trying reconnet..."
while True:
with t:
# data = subFromVeristand(dev)
data = [140, 10, 130, 50, 50, 130, 50, 130, 50]
print data
if not arduinoSer._isOpen:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
print e, ", trying reconnect..."
try:
if arduinoSer._isOpen:
arduinoSer.write(dataSend(data))
dataFromArduino = dataRead(arduinoSer)
print dataFromArduino
except serial.serialutil.SerialException, e:
arduinoSer.close()
try:
if dataFromArduino:
pubToVeristand(dev, dataFromArduino)
except UnboundLocalError:
pass
def main():
Arduino_ser = serial.Serial(port='COM9',
baudrate=9600,
bytesize=8,
parity='N',
stopbits=1,
timeout=1)
t = PeriodTimer(0.1)
t.start()
data = 0
while True:
with t:
# data += 1
# if data > 254:
# data = 0
data = [10, 130]
Arduino_ser.write(send_data(data))
print data
read_data(Arduino_ser)
def main():
# arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
arduinoSer = serial.serial_for_url(
arduinoUrl, baudrate=115200, do_not_open=True, timeout=1)
dev = MsgDevice()
dev.open()
dev.pub_bind(msgPubBind)
dev.sub_connect(msgSubConnect)
dev.sub_add_url('motorSpeed')
dev.sub_add_url('rudderAng')
dev.sub_add_url('sailAng')
t = PeriodTimer(0.1)
t.start()
try:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
arduinoSer.close()
print e, ", trying reconnet..."
while True:
with t:
data = subFromVeristand(dev)
# data = [100, 90, 0]
print "SEND: ", data
if not arduinoSer._isOpen:
try:
arduinoSer.open()
except serial.serialutil.SerialException, e:
print e, ", trying reconnect..."
try:
if arduinoSer._isOpen:
arduinoSer.write(dataSend(data))
dataFromArduino = dataRead(arduinoSer)
print "RECV: ", dataFromArduino
except serial.serialutil.SerialException, e:
arduinoSer.close()
try:
if dataFromArduino:
pubToVeristand(dev, dataFromArduino)
except UnboundLocalError:
pass
def thread(self, points, costfunc):
self.animation_initialize(points)
timer = PeriodTimer(0.1)
i = 0
cost = []
while True:
try:
with timer:
state = self.c.getNEData()
cost.append(costfunc(state))
adata = [
state['x'], state['y'], state['u'], state['v'],
state['phi'], state['alpha'], state['lm'], state['rm']
]
self.animate(adata, i, np.mean(cost))
i += 1
except:
print('Error happened in drawing!!!!')
break
def main(args):
USE_PZH = args.use_pzh
PP = args.pp
DWA = args.dwa
# initialize
interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH)
pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1)
u = np.array([0.0, 0.0]) # speed, yaw speed
config = Config()
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
# 需要根据实际出发点赋值
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# goal position [x(m), y(m)]
goal = np.array([-90, 3])
# obstacles
obstacles = []
csv_file = csv.writer(
open(
"log-{:s}.txt".format(
time.strftime("%m-%d-%H-%M", time.localtime())), 'w'))
csv_file.writerow([
"timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed',
'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track', 'target.posx',
'target.posy', 'target.yaw', 'target.yaw_speed', 'target.hspeed',
'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor',
'right_motor'
])
t = PeriodTimer(0.5)
t.start()
try:
while True:
with t:
self_state = interface001.receive('gps.posx', 'gps.posy',
'ahrs.yaw', 'ahrs.yaw_speed',
'gps.hspeed', 'gps.stdx',
'gps.stdy', 'gps.track')
target_state = interface002.receive('gps.posx', 'gps.posy',
'ahrs.yaw',
'ahrs.yaw_speed',
'gps.hspeed', 'gps.stdx',
'gps.stdy', 'gps.track')
obstacles.append([target_state[POS_X],
target_state[POS_Y]]) # [num_obstacles, 2]
# TODO 为什么把对手船也当作obstacle来看待？
# TODO 而且如果你用我的数据，这个地方不用做处理吗？
if USE_PZH:
assert pzhdata is not None
lidar_data = pzhdata.receive()
if lidar_data["terminated"]:
print(
"Peng Zhenghao's program is terminated. For safety we close this program."
)
break
target = lidar_data["target"]
if not target:
# TODO 你的代码是完全不支持发呆状态的。所以如果target没有指定会发生什么事情？
print("No Target Specified!")
# continue TODO 也许可以用continue跳过后面的所有东西，但这样可能有意外后果。
else:
goal = lidar_data[target] # goal = [x, y]
for key, centroid in lidar_data.items():
if key.startswith(
"Object") and centroid and key != target:
obstacles.append(centroid)
obstacles = trans_coord(
[self_state[POS_X], self_state[POS_Y]],
self_state[YAW], np.array(obstacles))
if PP:
# TODO 所以如果你要用我的target坐标来做pure pursuit呢？
target_angle, dist = pure_pursuit(self_state, target_state)
output = pid.compute(self_state[YAW], target_angle)
# TODO 你确定 yaw 和 target angle的零点与单位（rad or degree）一样？
elif DWA:
x = [
self_state[POS_X], self_state[POS_Y], self_state[YAW],
self_state[SPD], self_state[YAW_SPEED]
]
u, ltraj = dwa_control(x, u, config, goal, obstacles)
ideal_angle = self_state[YAW] + u[1] * 0.1
output = pid.compute(self_state[YAW], ideal_angle)
else:
output = 0
################## 此处往下没有问题 #######################
# dead band
output = 0 if abs(output) < 5 else output
# 本船距离目标的距离
dist = math.sqrt((self_state[POS_X] - goal[0])**2 +
(self_state[POS_Y] - goal[1])**2)
if dist <= 3:
average = baseline[0]
elif dist <= 10:
average = baseline[1]
else:
average = baseline[2]
left_motor, right_motor = (average +
output / 2), average - output / 2
left_motor = -rpm_limit(left_motor)
right_motor = rpm_limit(right_motor)
print(
'self ship state: ',
'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format(
self_state[POS_X], self_state[POS_Y], self_state[YAW],
self_state[SPD], left_motor, right_motor))
print(
'target ship state: ',
'posx:{}, posy: {}，distance:{}'.format(
target_state[POS_X], target_state[POS_Y], dist))
interface001.Motor_send(left_motor, right_motor)
csv_file.writerow([time.time()] + self_state + target_state +
[dist, left_motor, right_motor])
finally:
interface001.Motor_send(0, 0)
time.sleep(0.1)
interface001.dev.close()
interface002.dev.close()
csv_file.close()
print('everything closed')
# def b2a_little(bytes):
# return array.array('d', bytes)
# value=a2b_little(default_values)
# print(b2a_little(value)[0])
# print(len(17.0))
from msgdev import MsgDevice,PeriodTimer
from math import sin,cos
if __name__=="__main__":
dwa = MsgDevice()
dwa.open()
dwa.pub_bing('tcp://0.0.0.0:55010')
interval = 0.3
t = PeriodTimer(interval)
t.start()
try:
while True:
with t:
except KeyboardInterrupt:
print('close')
dwa.dev.close()
def main():
# auv = auv_initialize()
auv = MsgDevice()
auv.open()
auv.pub_bind('tcp://0.0.0.0:55004')
# Pure Pursuit initialize
target_angle = 0
dist = 0
# DWA initialize
configDWA = ConfigDWA()
dynamic_window = DWA(configDWA)
# initial state [x(m), y(m), yaw(rad), speed(m/s), yaw_speed(rad/s)]
# init_state = np.array([0, 10, 45 * pi / 180, 0.0, 0.0])
init_state = np.array([0, 0, 0, 0.0, 0.0])
# goal position [x(m), y(m)]
predefined_goal = np.array([90.0, 80.0])
goal = predefined_goal
# obstacles [x(m) y(m), spd(m/s), yaw]
# static_obstacles = np.array([[100.0, 70.0, 0.0, 0.0],
# [30.0, 39.0, 0, 0.0],
# [30.0, 70.0, 0.0, 0.0],
# [50.0, 70.0, 0.0, 0.0],
# [80, 70, 0, 0],
# [60, 40, 0, 0],
# [40, 40, 0, 0],
# [70, 80, 0, 0],
# [20.0, 20.0, 0.0, 0.0],
# ])
static_obstacles = np.array([
[90.0, 60.0, 0.0, 0.0],
[60.0, 70.0, 0, 0.0],
[80.0, 60.0, 0.0, 0.0],
[50.0, 70.0, 0.0, 0.0],
[80, 70, 0, 0],
[60, 40, 0, 0],
[40, 50, 0, 0],
[30, 60, 0, 0],
[20.0, 30.0, 0.0, 0.0],
])
moving_obstacles = np.array([[20, 10, 0.7, 2], [60, 30, 0.2, 2]])
u = np.array([0.0, 0.0])
# RRT* initialize
rrt_star = RRTStar(start=[init_state[0], init_state[1]],
goal=goal,
rand_area=[0, 100],
obstacle_list=static_obstacles,
expand_dis=5.0,
path_resolution=1.0,
goal_sample_rate=10,
max_iter=100,
connect_circle_dist=50.0)
# path = rrt_star.planning(animation=True, search_until_max_iter=True)
# path_index = -1
# localgoal = path[path_index]
# time.sleep(3)
traj = [init_state]
best_traj = None
state = init_state
interval = 0.1 # Simulation time step
pid_spd = PID(kp=2500.0,
ki=100.0,
kd=0,
minout=-1000,
maxout=1000,
sampleTime=interval)
pid_yawspd = PID(kp=15000,
ki=500.0,
kd=10,
minout=-1500,
maxout=1500,
sampleTime=interval)
pid_yaw = PID_angle(kp=800,
ki=3,
kd=10,
minout=-1500,
maxout=1500,
sampleTime=interval)
t = PeriodTimer(interval)
t.start()
i = 0
try:
while True:
with t:
start_time = time.perf_counter()
if i < 10000:
i += 1
else:
i = 0
goal = predefined_goal
obstacle = np.vstack((moving_obstacles, static_obstacles))
min_dist = 100**2
for ob_single in moving_obstacles:
dist = (state[0] - ob_single[0])**2 + (state[1] -
ob_single[1])**2
if dist < min_dist:
min_dist = dist
# to_localgoal_dist_square = (state[POSX] - localgoal[0]) ** 2 + (state[POSY] - localgoal[1]) ** 2
# if to_localgoal_dist_square <= configDWA.robot_radius_square:
# path_index -= 1
# if abs(path_index) < len(path):
# localgoal = path[path_index]
# else:
# localgoal = goal
# if min_dist > 8 ** 2:
if 0:
best_traj = None
target_angle, dist = pure_pursuit(state, localgoal)
if target_angle > pi:
target_angle -= 2 * pi
elif target_angle < -pi:
target_angle += 2 * pi
output = pid_yaw.compute(
state[2], target_angle) # yaw, target_angle unit: rad
# dead band
diff = 0 if abs(output) < 5 else output
# use different speed based on distance
if dist <= 3:
average = 800
elif dist <= 10:
average = 1000
else:
average = 1200
print('Pure Pursuit, Current path point: ', -path_index,
"/", len(path))
else:
u, best_traj = dynamic_window.update(
state, u, goal, obstacle)
u = [1, 0.1]
# Forward velocity
average = pid_spd.compute(state[3], u[0])
average = 0 if abs(average) < 5 else average
# Yaw angle velocity
diff = pid_yawspd.compute(state[4], u[1])
diff = 0 if abs(diff) < 5 else diff
print('Dynamic Window Approach')
# which path point to follow when switch to the pure pursuit
# path_index = get_nearest_path_index(path, state)-len(path)
if min_dist < 3**2:
print('Collision!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
print(
info_format.format(i=i,
real_spd=state[3],
target_spd=u[0],
real_yaw=state[2],
target_yaw=target_angle,
real_yawspd=state[4],
target_yawspd=u[1],
average_output=average,
output_diff=diff,
calc_time=time.perf_counter() -
start_time))
if show_animation:
traj.append(state.copy())
# plot(best_traj, state, goal, obstacle, path, traj)
plot2(best_traj, state, goal, obstacle, traj)
to_goal_dist_square = (state[POSX] -
goal[0])**2 + (state[POSY] - goal[1])**2
if to_goal_dist_square <= configDWA.robot_radius_square:
time.sleep(3)
print("Goal!!")
break
# Simulation
# state = trimaran_model(state, left, right, interval)
state = AUV_model(state, average, -diff, interval)
state = apply_noise(state)
moving_obstacles = update_obstacle(moving_obstacles, interval)
auv.pub_set1('posx', state[0])
auv.pub_set1('posy', state[1])
auv.pub_set1('speed', state[3])
auv.pub_set1('yaw', state[2])
auv.pub_set1('yawspd', state[4])
auv.pub_set1('speed_tar', u[0])
auv.pub_set1('yaw_tar', target_angle)
auv.pub_set1('yawspd_tar', u[1])
finally:
time.sleep(interval)
print('everything closed')
# self.capture()
# self.parse()
self.publish()
def close(self):
self.s.close()
print('VLP-16 Disconnected.')
if __name__ == "__main__":
try:
dev = MsgDevice()
dev.open()
devinitial(dev)
vlp = VLP(dev)
t = PeriodTimer(t_refresh)
t.start()
while True:
with t:
print(time.strftime('%Y-%m-%d-%H-%M-%S', time.localtime()))
vlp.update()
except KeyboardInterrupt:
vlp.close()
dev.close()
except Exception:
vlp.close()
dev.close()
raise
else:
vlp.close()
ret["target"] = target
# return ret
return pickle.dumps(ret)
def build_push_detection_dev(push_detection_dev):
push_detection_dev.open()
# push_detection_dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口
push_detection_dev.sub_connect("tcp://192.168.0.8:55019")
push_detection_dev.sub_add_url("det.data", [-100] * 7)
if __name__ == "__main__":
push_detection_dev = MsgDevice()
build_push_detection_dev(push_detection_dev)
from msgdev import PeriodTimer
t = PeriodTimer(0.1)
try:
while True:
with t:
data = push_detection_dev.sub_get("det.data")
print(data)
# print(data, pickle.loads(data))
finally:
push_detection_dev.pub_set("data", pickle.dumps(None))
push_detection_dev.close()
logging.info("Everything Closed!")
except ValueError:
self.logger.warning('invalid novatel cksum: ' + ps[1])
return False
if cksum != novatel_crc32(self.header + ';' + self.content):
self.logger.warning('invalid novatel cksum: ' + ps[1])
return False
return True
if __name__ == "__main__":
gps_url = URL_BaseStation
try:
gps = GNSS(gps_url)
gps.write(INIT_COMMANDS)
t = PeriodTimer(0.2)
t.start()
aver_x = 0
aver_y = 0
aver_z = 0
n = 1
while n < 300:
with t:
gps.update()
if gps.posx != 0:
aver_x = 1.0 * (n - 1) / n * aver_x + 1.0 / n * gps.posx
print(aver_x)
aver_y = 1.0 * (n - 1) / n * aver_y + 1.0 / n * gps.posy
aver_z = 1.0 * (n - 1) / n * aver_z + 1.0 / n * gps.posz
n += 1
print('Fixed Position is:', aver_x, aver_y, aver_z)
from PID import PID
from communicater import Communicater
from decisionMaker import Maker
# from drawer import Drawer
from msgdev import PeriodTimer
kp = 800
ki = 3
kd = 10
points = [[-42, 46], [-17, -34], [-85, -24], [-85, 46], [-42, 46]]
if __name__ == '__main__':
c = Communicater()
pid = PID(kp=kp, ki=ki, kd=kd, minout=-2500, maxout=500, sampleTime=0.1)
timer = PeriodTimer(0.1)
maker = Maker(points)
cost = 0
data = []
i = 0
while True:
try:
with timer:
state = c.getNEData()
ideal_angle = maker.getDecision(state)
cost += maker.getCost(state)
if ideal_angle == -1000:
c.upload(0, 0)
print('试验成功！')
from msgdev import MsgDevice, PeriodTimer
import numpy as np
if __name__=="__main__":
dev=MsgDevice()
dev.open()
dev.pub_bind('tcp://0.0.0.0:55005')
data = np.loadtxt('log-05-05-17-07.txt', skiprows=1, usecols=(3,4), delimiter=',')
i = 0
t=PeriodTimer(0.1)
t.start()
try:
while True:
with t:
dev.pub_set1('ahrs.yaw', data[i,0])
dev.pub_set1('ahrs.yaw_speed', data[i,1])
print('ahrs.yaw', data[i,0], 'ahrs.yaw_speed', data[i,1])
i += 1
if i == len(data): i = 0
except (KeyboardInterrupt, Exception) as e:
print('closed')
finally:
pass
def main():
USE_PZH = True
# initialize
interface001, interface002, pzhdata = ship_initialize(True, True, True)
t = PeriodTimer(0.2)
diff_x_average_gps = MovingAverage(100)
diff_y_average_gps = MovingAverage(100)
diff_x_average_lidar = MovingAverage(100)
diff_y_average_lidar = MovingAverage(100)
# t.start()
cnt = 0
end = 200
try:
while True:
with t:
self_state = interface001.receive('gps.posx', 'gps.posy',
'ahrs.yaw', 'ahrs.yaw_speed',
'gps.hspeed', 'gps.stdx',
'gps.stdy', 'gps.track')
target_state = interface002.receive('gps.posx', 'gps.posy',
'ahrs.yaw',
'ahrs.yaw_speed',
'gps.hspeed', 'gps.stdx',
'gps.stdy', 'gps.track')
assert pzhdata is not None
lidar_data = pzhdata.receive()
if lidar_data["terminated"]:
print(
"Peng Zhenghao's program is terminated. For safety we close this program."
)
break
target = lidar_data["target"]
if not target:
print("No Target Specified!")
continue
else:
cnt += 1
# print("Current CNT")
goal = lidar_data[target] # goal = [x, y]
diff_x = target_state[POS_X] - self_state[POS_X]
diff_y = target_state[POS_Y] - self_state[POS_Y]
diff_x_average_gps.update(diff_x)
diff_y_average_gps.update(diff_y)
diff_x_average_lidar.update(goal[0])
diff_y_average_lidar.update(goal[1])
phi2 = -atan2(diff_y_average_gps.avg,
diff_x_average_gps.avg) - pi / 2
phi1 = atan2(diff_y_average_lidar.avg,
diff_x_average_lidar.avg)
out = phi1 + phi2 - pi / 2
# offset = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \
# atan2(diff_y_average_gps.avg, diff_x_average_gps.avg)
print("[CNT {}] Current GPS ({}, {}), LiDAR ({}, {}). \
ph1{}, ph2 {}, out {} ({} deg).".format(
cnt, diff_x_average_gps.avg, diff_y_average_gps.avg,
diff_x_average_lidar.avg, diff_y_average_lidar.avg,
phi1, phi2, out, out * 180 / pi))
if cnt >= end:
break
finally:
import pickle
import time
def get_formatted_time(timestamp=None):
if not timestamp:
return time.strftime('%Y-%m-%d_%H-%M-%S', time.localtime())
else:
return time.strftime('%Y-%m-%d_%H-%M-%S',
time.localtime(timestamp))
if diff_y_average_lidar.avg is not None:
phi2 = -atan2(diff_y_average_gps.avg,
diff_x_average_gps.avg) - pi / 2
phi1 = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg)
out = phi1 + phi2 - pi / 2
out = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \
atan2(diff_y_average_gps.avg, diff_x_average_gps.avg)
pickle.dump(
{
"offset": out,
"timestamp": time.time(),
"time": get_formatted_time()
}, open("offset.pkl", "wb"))
print("Data have saved to offset.pkl")
else:
print("Data is not received.")
time.sleep(0.5)
interface001.dev.close()
interface002.dev.close()
pzhdata.dev.close()
print('dev closed')
def main():
# USE_PZH = args.use_pzh
# PP = args.pp
# DWA = args.dwa
# initialize
interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH)
pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1)
u = np.array([0.0, 0.0]) # speed, yaw speed
config = Config()
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
# 需要根据实际出发点赋值
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# goal position [x(m), y(m)]
goal = np.array([-90, 3])
predefiend_goal = goal.copy()
# obstacles
obstacles = []
csv_source = open("log-{:s}.txt".format(time.strftime("%m-%d-%H-%M", time.localtime())), 'w')
csv_file = csv.writer(csv_source)
csv_file.writerow(
["timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed',
'gps.stdx',
'gps.stdy', 'gps.track', 'target.posx', 'target.posy', 'target.yaw', 'target.yaw_speed',
'target.hspeed', 'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor',
'right_motor'])
# 计算频率
interval = 0.5
t = PeriodTimer(interval)
t.start()
try:
while True:
with t:
self_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
'ahrs.yaw_speed', 'gps.hspeed',
'gps.stdx', 'gps.stdy', 'gps.track')
target_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
'ahrs.yaw_speed', 'gps.hspeed',
'gps.stdx', 'gps.stdy', 'gps.track')
# target_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
# 'ahrs.yaw_speed', 'gps.hspeed',
# 'gps.stdx', 'gps.stdy', 'gps.track')
# self_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
# 'ahrs.yaw_speed', 'gps.hspeed',
# 'gps.stdx', 'gps.stdy', 'gps.track')
if USE_PZH:
assert pzhdata is not None
lidar_data = pzhdata.receive()
if lidar_data["terminated"]:
print(
"Peng Zhenghao's program is terminated. For safety we close this program.")
break
#### FOR DEBUG
# print("Peng Zehgnhao data: ", lidar_data)
# continue
target = lidar_data["target"]
if not target: # 如果没有搜索到target, 则用初始化的goal作为目标点
# TODO 所以如果target没有指定会发生什么事情？
print("No Target Specified! Heading to predefiend_goal")
goal = predefiend_goal
else:
goal = target # goal = [x, y]
for key, centroid in lidar_data.items():
if key.startswith("Object") and centroid and key != target:
obstacles.append(centroid)
obstacles = trans_coord([self_state[POS_X], self_state[POS_Y]],
self_state[YAW],
np.array(obstacles))
assert obstacles.shape[1] == 2
obstacles = obstacles.tolist()
else: # 如果不用PZH, 则使用避障目标船的GPS
obstacles = [[target_state[POS_X], target_state[POS_Y]]] # [num_obstacles, 2]
if PP:
if USE_PZH:
target_angle, dist = pure_pursuit(self_state, goal)
else:
target_angle, dist = pure_pursuit(self_state, target_state)
output = pid.compute(self_state[YAW], target_angle) # yaw, target_angle unit: rad
elif DWA:
x = [self_state[POS_X], self_state[POS_Y], self_state[SPD_DIR], self_state[SPD],
self_state[YAW_SPEED]]
u, ltraj = dwa_control(x, u, config, goal, obstacles)
ideal_angle = self_state[SPD_DIR] + u[1] * interval
output = pid.compute(self_state[SPD_DIR], ideal_angle) * 50 # 输出太小???
print('output', output, 'self_state[SPD_DIR]', self_state[SPD_DIR], 'ideal_angle', ideal_angle)
# 本船距离目标的距离
dist = math.sqrt((self_state[POS_X] - goal[0]) ** 2 +
(self_state[POS_Y] - goal[1]) ** 2)
else:
output, dist = 0, 0
# dead band
diff = 0 if abs(output) < 5 else output
# 根据距离给速度分级 # TODO 改为速度控制器
if dist <= 3:
average = baseline[0]
elif dist <= 10:
average = baseline[1]
else:
average = baseline[2]
left_motor, right_motor = (average + diff / 2), (average - diff / 2) # 除以2, 转弯时超调小
left_motor = -rpm_limit(left_motor)
right_motor = rpm_limit(right_motor)
print('self ship state: ',
'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format(self_state[POS_X],
self_state[POS_Y],
self_state[SPD_DIR],
self_state[SPD],
left_motor,
right_motor))
print('target ship state: ',
'posx:{}, posy: {}，distance:{}'.format(target_state[POS_X],
target_state[POS_Y], dist))
print('goal', goal, 'obstacles', obstacles)
interface001.Motor_send(left_motor, right_motor)
csv_file.writerow(
[time.time()] + self_state + target_state + [dist, left_motor,
right_motor])
finally:
interface001.Motor_send(0, 0)
time.sleep(interval)
interface001.dev.close()
interface002.dev.close()
pzhdata.dev.close()
csv_source.close()
print('everything closed')