class Interface(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, ekf_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr+':'+ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr+':'+gnss_port)
self.dev.sub_add_url('gps.time')
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.satn')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
self.dev.sub_connect(sub_addr+':'+ekf_port)
self.dev.sub_add_url('USV150.state', default_values=[0,0,0,0,0,0])
self.dev.pub_bind('tcp://0.0.0.0:'+motor_port)
def receive1(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get(i))
return data
def Motor_send(self, left_motor, right_motor):
print ('left_motor, right_motor', left_motor, right_motor)
self.dev.pub_set1('pro.left.speed', left_motor)
self.dev.pub_set1('pro.right.speed', right_motor)
def pointSend(self, pose):
self.dev.pub_set('target.point', pose)
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():
# 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 main():
logger = console_logger()
dev = MsgDevice()
gnss = GNSS(GNSS_URL, BAUDRATE, TIMEOUT, INIT_COMMANDS, logger)
gnss.set_origin(ORIGIN_LAT, ORIGIN_LON)
attrs = (
# weektime in seconds
"time",
# position x/y/z in m
"posx",
"posy",
"posz",
# standard deviation x/y/z in m
"stdx",
"stdy",
"stdz",
# num of satellites
"satn",
# horizontal speed (m/s), horizontal speed direction (rad, north is 0), vertical speed (m/s)
"hspeed",
"track",
"vspeed")
try:
dev.open()
for ep in MSG_PUB_BINDS:
dev.pub_bind(ep)
for ep in MSG_PUB_CONNS:
dev.pub_connect(ep)
while 1:
gnss.update()
for attr in attrs:
dev.pub_set1('gps.' + attr, getattr(gnss, attr, 0))
except KeyboardInterrupt:
pass
finally:
gnss.close()
dev.close()
class Interface(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
self.dev.pub_bind('tcp://0.0.0.0:' + motor_port)
def Sensor_receive(self, *args):
package = []
for i in args:
package.append(self.dev.sub_get1(i))
return package
def Motor_send(self, left_motor, right_motor, autoctrl):
self.dev.pub_set1('js.left.speed', left_motor)
self.dev.pub_set1('js.right.speed', right_motor)
self.dev.pub_set1('js.autoctrl', autoctrl)
class Interface_msg_pub(object):
def __init__(self, sub_addr, ahrs_port, gnss_port, motor_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + ahrs_port)
self.dev.sub_add_url('ahrs.roll')
self.dev.sub_add_url('ahrs.pitch')
self.dev.sub_add_url('ahrs.yaw')
self.dev.sub_add_url('ahrs.roll_speed')
self.dev.sub_add_url('ahrs.pitch_speed')
self.dev.sub_add_url('ahrs.yaw_speed')
self.dev.sub_add_url('ahrs.acce_x')
self.dev.sub_add_url('ahrs.acce_y')
self.dev.sub_add_url('ahrs.acce_z')
self.dev.sub_connect(sub_addr + ':' + gnss_port)
self.dev.sub_add_url('gps.time')
self.dev.sub_add_url('gps.posx')
self.dev.sub_add_url('gps.posy')
self.dev.sub_add_url('gps.posz')
self.dev.sub_add_url('gps.stdx')
self.dev.sub_add_url('gps.stdy')
self.dev.sub_add_url('gps.stdz')
self.dev.sub_add_url('gps.satn')
self.dev.sub_add_url('gps.hspeed')
self.dev.sub_add_url('gps.vspeed')
self.dev.sub_add_url('gps.track')
self.dev.pub_bind('tcp://0.0.0.0:' + motor_port)
def receive(self, *args):
data = []
for i in args:
data.append(self.dev.sub_get1(i))
return data
def Motor_send(self, left_motor, right_motor):
self.dev.pub_set1('pro.left.speed', left_motor)
self.dev.pub_set1('pro.right.speed', right_motor)
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
class MIO_TRANS():
def __init__(self):
self.dev_connect_flag = 0
self.datashow_count = 0
self.logger = console_logger('MIO-5251')
self.dev_init()
self.ser_init()
self.Servo_init()
self.fst = struct.Struct('!4H')
def dev_init(self):
self.jsdev = MsgDevice()
self.dev = MsgDevice()
self.tmdev = MsgDevice()
self.jsdev.open()
self.dev.open()
self.tmdev.open()
try:
self.tmdev.sub_connect('tcp://127.0.0.1:55006')
self.logger.info('tmdev sub connect tcp://127.0.0.1:55006')
if Local_test is True:
self.dev.sub_connect(MSG_SUB_CONNECTS[0])
self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[0])
self.dev.pub_bind(MSG_PUB_BINDS)
self.logger.info('dev pub bind ' + MSG_PUB_BINDS)
self.jsdev.sub_connect(MSG_SUB_CONNECTS[2])
self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[2])
else:
self.dev.sub_connect(MSG_SUB_CONNECTS[1])
self.logger.info('dev sub connect ' + MSG_SUB_CONNECTS[1])
self.dev.pub_bind(MSG_PUB_BINDS)
self.logger.info('dev pub bind ' + MSG_PUB_BINDS)
self.jsdev.sub_connect(MSG_SUB_CONNECTS[3])
self.logger.info('jsdev sub connect ' + MSG_SUB_CONNECTS[3])
self.jsdev.sub_add_url('js.Autoctrl')
self.tmdev.sub_add_url('Transmitter_flag')
except (zmq.error.ZMQError):
self.dev.close()
self.jsdev.close()
self.logger.info('Address already in use')
raise
def ser_init(self):
if not Serial_on:
return
try:
self.Arduino_ser = serial.Serial(Arduino_port_addr,
baudrate=9600,
timeout=1)
self.logger.info(self.Arduino_ser.portstr + ' open successfully!')
self.Arduino_ser.flushInput()
self.Arduino_read = Arduino_READ(self.Arduino_ser, self.dev)
except (serial.serialutil.SerialException, OSError):
self.dev.close()
self.jsdev.close()
self.logger.info('could not open port: ' + Arduino_port_addr)
raise
def Servo_init(self):
self.motor = Servo('Motor_speed', 30, 0, self.dev, self.jsdev,
self.tmdev)
self.rudder = Servo('Rudder_ang', 40, 4, self.dev, self.jsdev,
self.tmdev)
self.mainsail = Servo('Mainsail_ang', 80, 0, self.dev, self.jsdev,
self.tmdev)
self.foresail = Servo('Foresail_ang', 70, 0, self.dev, self.jsdev,
self.tmdev)
def update(self):
self.MIO_pub_sub()
self.Arduino_update()
if not Data_show:
return
self.DataInfoShow()
def MIO_pub_sub(self):
self.js_Autoctrl = int(self.jsdev.sub_get1('js.Autoctrl'))
self.dev.pub_set1('js.Autoctrl', self.js_Autoctrl)
self.dev_connect_flag = not self.dev_connect_flag
self.dev.pub_set1('dev_connect_flag', self.dev_connect_flag)
self.Transmitter_flag = self.tmdev.sub_get1('Transmitter_flag')
self.motor.update(self.js_Autoctrl, self.Transmitter_flag)
self.rudder.update(self.js_Autoctrl, self.Transmitter_flag)
self.mainsail.update(self.js_Autoctrl, self.Transmitter_flag)
self.foresail.update(self.js_Autoctrl, self.Transmitter_flag)
global Data_show
if self.Transmitter_flag == 1:
Data_show = False
def Arduino_update(self):
if not Serial_on:
return
self.Arduino_read.update()
self.Arduino_write()
def Arduino_write(self):
header = '\xff\x01'
tmp = self.fst.pack(self.motor.c_data, self.rudder.c_data,
self.mainsail.c_data, self.foresail.c_data)
crc_code = struct.pack('!H', crc16(tmp))
tmp = header + tmp
tmp = tmp + crc_code
self.Arduino_ser.write(tmp)
def DataInfoShow(self):
self.datashow_count += 1
if self.datashow_count < 15:
return
self.datashow_count = 0
self.logger.info('''MIO received data from bank:
Autoctrl, Motor, Rudder, Mainsail, Foresail: ''' + str([
self.js_Autoctrl, self.motor.r_data, self.rudder.r_data,
self.mainsail.r_data, self.foresail.r_data
]))
if not Serial_on:
return
self.logger.info('''Arduino to MIO data:)
dogvane, mainsail, voltage, arduino_receive_flag: ''' + str([
self.Arduino_read.dogvane, self.Arduino_read.mainsail,
self.Arduino_read.voltage, self.Arduino_read.arduino_receive_flag
]))
self.logger.info('''MIO to Arduino data: )
motor_pwm, rudder_pwm, mainsail_pwm, foresail_pwm: ''' + str([
self.motor.c_data, self.rudder.c_data, self.mainsail.c_data,
self.foresail.c_data
]))
print
class Communicater(object):
def __init__(self, upload=True):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect('tcp://192.168.1.150:55003') # 下载端口
self.upload_flag = upload
if upload:
self.dev.pub_bind('tcp://0.0.0.0:55010') # 上传端口
self.dev.sub_add_url('')
self.dev_gnss = MsgDevice()
self.dev_gnss.open()
self.dev_gnss.sub_connect('tcp://192.168.1.150:55004')
self.dev_ahrs = MsgDevice()
self.dev_ahrs.open()
self.dev_ahrs.sub_connect('tcp://192.168.1.150:55005')
self.dev_volt = MsgDevice()
self.dev_volt.open()
self.dev_volt.sub_connect('tcp://192.168.1.150:55006')
self.data_down = []
self.data_up = [None, None]
self.file_name = './data/' + time.strftime("%Y-%m-%d__%H:%M",
time.localtime())
self.f = open(self.file_name + '.csv', 'w')
self.writer = csv.writer(self.f)
self.dev_gnss.sub_add_url('gps.posx'),
self.dev_gnss.sub_add_url('gps.posy'),
self.dev_gnss.sub_add_url('gps.posz'),
self.dev_gnss.sub_add_url('gps.hspeed'),
self.dev_gnss.sub_add_url('gps.vspeed'),
self.dev_gnss.sub_add_url('gps.track')
self.dev_ahrs.sub_add_url('ahrs.yaw'),
self.dev_ahrs.sub_add_url('ahrs.yaw_speed'),
self.dev_ahrs.sub_add_url('ahrs.acce_x'),
self.dev_ahrs.sub_add_url('ahrs.acce_y'),
self.dev_ahrs.sub_add_url('ahrs.acce_z'),
self.dev.sub_add_url('left.Motor_SpeedCalc')
self.dev.sub_add_url('right.Motor_SpeedCalc')
self.dev_volt.sub_add_url('voltage')
def download(self):
gps = [
self.dev_gnss.sub_get1('gps.posx'),
self.dev_gnss.sub_get1('gps.posy'),
self.dev_gnss.sub_get1('gps.posz'),
self.dev_gnss.sub_get1('gps.hspeed'),
self.dev_gnss.sub_get1('gps.vspeed'),
self.dev_gnss.sub_get1('gps.track')
]
ahrs = [
self.dev_ahrs.sub_get1('ahrs.yaw'),
self.dev_ahrs.sub_get1('ahrs.yaw_speed'),
self.dev_ahrs.sub_get1('ahrs.acce_x'),
self.dev_ahrs.sub_get1('ahrs.acce_y'),
self.dev_ahrs.sub_get1('ahrs.acce_z'),
]
l_m = self.dev.sub_get1('left.Motor_SpeedCalc')
r_m = self.dev.sub_get1('right.Motor_SpeedCalc')
volt = self.dev_volt.sub_get1('voltage')
return gps, ahrs, l_m, r_m, volt
def getNEData(self):
"""
:return: x,y,u,v,yaw,omega in NE sys.
"""
gps, ahrs, l_m, r_m, volt = self.download()
yaw = ahrs[0] - 5 * math.pi / 180
U = gps[3] * math.cos(gps[5]) # 北东系速度
V = gps[3] * math.sin(gps[5])
self.data_down = [gps[0], gps[1], U, V, yaw, ahrs[1]]
return {
'x': gps[0],
'y': gps[1],
'u': U,
'v': V,
'phi': yaw,
'alpha': ahrs[1],
'lm': l_m,
'rm': r_m
}
def upload(self, left, right):
if self.upload_flag:
self.dev.pub_set1('pro.left.speed', left)
self.dev.pub_set1('pro.right.speed', right)
self.data_up = [left, right]
print('uploaded', left, right)
else:
self.data_up = [-1, -1]
print('upload disabled')
def record(self):
self.writer.writerow(self.data_down + self.data_up)
print('Saved: ', self.data_down + self.data_up)
def __del__(self):
self.upload(0, 0)
self.dev.close()
self.f.close()
self.dev.pub_set1(self.prefix+'.roll',self.roll)
self.dev.pub_set1(self.prefix+'.pitch',self.pitch)
self.dev.pub_set1(self.prefix+'.yaw',self.yaw)
self.dev.pub_set1(self.prefix+'.roll_speed',self.roll_speed)
self.dev.pub_set1(self.prefix+'.pitch_speed',self.pitch_speed)
self.dev.pub_set1(self.prefix+'.yaw_speed',self.yaw_speed)
self.dev.pub_set1(self.prefix+'.acce_x',self.acce_x)
self.dev.pub_set1(self.prefix+'.acce_y',self.acce_y)
self.dev.pub_set1(self.prefix+'.acce_z',self.acce_z)
self.dev.pub_set1(self.prefix+'.devicestatus',self.devicestatus)
#AHRS_URL = 'COM3'
AHRS_URL = '/dev/serial/by-id/usb-Silicon_Labs_SBG_Systems_-_UsbToUart_001000929-if00-port0'
if __name__ == "__main__":
try:
dev_pro = MsgDevice()
dev_pro.open()
dev_pro.pub_bind('tcp://0.0.0.0:55005')
ahrs = AHRS(AHRS_URL,dev_pro)
while True:
ahrs.update()
except (KeyboardInterrupt, Exception) as e:
dev_pro.close()
ahrs.close()
raise
finally:
pass
master = modbus_rtu.RtuMaster(
serial.Serial(port=RTU_port,
baudrate=57600,
bytesize=8,
parity='E',
stopbits=1,
xonxoff=0))
master.set_timeout(1.0)
master.set_verbose(True)
#Connect to control program
dev_pro = MsgDevice()
dev_pro.open()
dev_pro.sub_connect(
'tcp://192.168.1.60:55002') #receive rpm from veristand
dev_pro.pub_bind('tcp://0.0.0.0:55003') #send information to veristand
# Connect to joystick
dev_joy = MsgDevice()
dev_joy.open()
dev_joy.sub_connect(
'tcp://192.168.1.60:55001') #receive rpm from joystick
dev_joy.sub_add_url('js.autoctrl')
left_motor = Motor(dev_pro, dev_joy, 'left', 0x01, master)
right_motor = Motor(dev_pro, dev_joy, 'right', 0x02, master)
t = PeriodTimer(0.1)
t.start()
while True:
with t:
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')
# 0 "timestamp",
# 1'gps.posx', 2'gps.posy', 3'ahrs.yaw', 4'ahrs.yaw_speed', 5'gps.hspeed',6'gps.stdx', 7'gps.stdy', 8'gps.track',
# 9'target.posx', 10'target.posy', 11'target.yaw', 12'target.yaw_speed',
# 13'target.hspeed', 14'target.stdx', 15'target.stdy', 16'target.track',
# 17'distance', 18'left_motor', 19 'right_motor'
USE_Fake_Target = True
USE_Fake_Obstacle1 = True
USE_Fake_Obstacle2 = True
if __name__ == "__main__":
if USE_Fake_Target:
target = MsgDevice()
target.open()
target.pub_bind('tcp://0.0.0.0:55205') #TLG002
target_data = np.loadtxt(
'fakedata4.txt'
) #, skiprows=1, usecols=(9, 10, 13, 16), delimiter=',')
if USE_Fake_Obstacle1:
ob1 = MsgDevice()
ob1.open()
ob1.pub_bind('tcp://0.0.0.0:55305')
ob1_data = np.loadtxt('fakedata5.txt')
if USE_Fake_Obstacle2:
ob2 = MsgDevice()
ob2.open()
ob2.pub_bind('tcp://0.0.0.0:55405')
ob2_data = np.loadtxt('fakedata6.txt')
from msgdev import MsgDevice, PeriodTimer
import numpy as np
if __name__=="__main__":
dev=MsgDevice()
dev.open()
#dev.pub_bind('tcp://0.0.0.0:55004') #TLG001
dev.pub_bind('tcp://0.0.0.0:55204') #TLG002
data = np.loadtxt('log-05-05-17-07.txt', skiprows=1, usecols=(1,2,5,8), delimiter=',')
i = 0
t=PeriodTimer(0.1)
t.start()
try:
while True:
with t:
dev.pub_set1('gps.posx', data[i,0])
dev.pub_set1('gps.posy', data[i,1])
dev.pub_set1('gps.hspeed', data[i,2])
dev.pub_set1('gps.track', data[i,3])
print('gps.posx', data[i,0], 'gps.posy', data[i,1], 'gps.hspeed', data[i,2], 'gps.track', data[i,3])
i += 1
if i == len(data): i = 0
except (KeyboardInterrupt, Exception) as e:
print('closed')
finally:
pass