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 __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])
if motor_port is not None:
self.dev.pub_bind('tcp://0.0.0.0:' + motor_port)
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')
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 __init__(self, sub_addr, pose_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + pose_port)
self.dev.sub_add_url('posx')
self.dev.sub_add_url('posy')
self.dev.sub_add_url('speed')
self.dev.sub_add_url('speed_tar')
self.dev.sub_add_url('yaw')
self.dev.sub_add_url('yaw_tar')
self.dev.sub_add_url('yawspd')
self.dev.sub_add_url('yawspd_tar')
def __init__(self, sub_addr, gnss_port):
self.dev = MsgDevice()
self.dev.open()
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')
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()
'-o',
type=float,
required=True,
help="The output value of gps_alignment.")
parser.add_argument("--fps", type=int, default=-1)
args = parser.parse_args()
setup_logger("INFO")
map = LidarMap(lidar_config, args.offset * np.pi / 180)
detector = Detector()
v = Visualizer("Detection",
detector_config["map_size"],
smooth=False,
max_size=1000)
push_detection_dev = MsgDevice()
build_push_detection_dev(push_detection_dev)
pull_lidar_dev = MsgDevice()
build_pull_lidar_dev(pull_lidar_dev)
if args.fps == -1:
args.fps = None
fps_timer = FPSTimer(force_fps=args.fps)
# parser.add_argument("--path", '-p', required=True, help="The path of target h5 file.")
# parser.add_argument("--save", '-s', action="store_true")
# parser.add_argument("--fps", type=int, default=-1)
# parser.add_argument("--start", type=int, default=0)
# args = parser.parse_args()
def __init__(self, sub_addr, object_port):
self.dev = MsgDevice()
self.dev.open()
self.dev.sub_connect(sub_addr + ':' + object_port)
self.dev.sub_add_url("det.data", [-100] * 7) # -100表示信号丢失，-99表示程序断了。
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')
def build_push_data(object_dict, target):
ret = {k: {"centroid": v["centroid"].tolist()} for k, v in object_dict.items()}
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()
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 setup_vlp(fake=False):
dev = MsgDevice()
dev.open()
devinitial(dev)
vlp = VLP(dev, fake)
return vlp
# return array.array('d', arr).tostring()
# 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()
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
# TRU_logger = modbus_tk.utils.create_logger("console")
try:
#Connect to the slave
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://127.0.0.1:55002')
dev_pro.pub_bind('tcp://0.0.0.0:55003')
# Connect to joystick
dev_joy = MsgDevice()
dev_joy.open()
dev_joy.sub_connect('tcp://127.0.0.1:55001')
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()
from msgdev import MsgDevice, PeriodTimer
import numpy as np
# 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()