def __init__(self):
self.kalman_pos = pos()
self.kalman_pos.x = 0
self.kalman_pos.y = 0
self.kalman_pos.z = 0
self.kalman_pos.yaw = 0
self.obstacle_pos = pos()
self.P = self.covariance(1000, 100)
self.A = np.array([[1, 0], [0, 1]])
self.H = np.identity(2)
self.R = self.covariance(1000, 100)
self.K = self.H
self.X = np.array([[self.kalman_pos.x], [self.kalman_pos.z]])
def __init__(self, node):
self.node = node
self.msg = Tags()
self.localization = localization()
self.position_0 = pos()
self.position_1 = pos()
self.pos = pos()
self.kalman_pos = pos()
self.kalman_pos.x = 0
self.kalman_pos.y = 0
self.kalman_pos.z = 0
self.kalman_pos.yaw = 0
self.obstacle_pos = pos()
self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]}
self.node.create_reader("/localization/tag", Tags, self.callback)
self.node.create_reader("/realsense/pose", Pose, self.posecallback)
self.node.create_reader("/chassis", Chassis, self.Chassiscallback)
self.writer = self.node.create_writer("/localization", localization)
self.writer_kalman = self.node.create_writer(
"/localization/kalman_filter", pos)
self.writer_obstacle = self.node.create_writer(
"/localization/obstacle", pos)
self.start_yaw = 0
self.init_flag = 0
self.obstacle_flag = 0
self.speed = 0
self.yaw = 0
self.kalman = kalman_filter()
while not cyber.is_shutdown():
time.sleep(0.05)
self.localization_with_odometer_calculation(
self.speed, self.yaw, 0.05)
# self.localization_with_kalman();
self.kalman.localization_with_kalman(self.speed, self.yaw)
self.writer.write(self.localization)
self.writer_kalman.write(self.kalman.kalman_pos)
if self.obstacle_flag:
self.obstacle_flag = 0
else:
self.obstacle_pos.x = -1
self.obstacle_pos.y = -1
self.obstacle_pos.z = -1
self.obstacle_pos.yaw = -1
self.writer_obstacle.write(self.kalman.obstacle_pos)
print(self.localization)
def __init__(self, node):
self.node = node
self.msg = Tags()
self.localization = localization()
self.position_0 = pos()
self.position_1 = pos()
self.pos = pos()
self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]}
self.node.create_reader("/localization/tag", Tags, self.callback)
self.node.create_reader("/realsense/pose", Pose, self.posecallback)
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.writer = self.node.create_writer("/localization", localization)
self.start_yaw = 0
self.init_flag = 0
self.speed = 0
self.yaw = 0
while not cyber.is_shutdown():
time.sleep(0.05)
self.localization_with_odometer_calculation()
self.writer.write(self.localization)
print(self.localization)
def __init__(self, node):
self.node = node
self.alpha = 0.01
self.maxTimes = 100
self.x = np.array([0.725, 0.7])
# base station matrix
self.xi = np.array([])
self.yi = np.array([])
# distance
self.di = np.array([])
#self.di = np.array([8, 9.303, 0, 4.75])
self.localization = localization()
self.pos = pos()
self.vel_head = 0
self.yaw = -3.1415926
self.direction = 0
self.x_now = 0.5
self.y_now = 2.2
self.velocity = 0
self.accelorator = 0
self.chassis = Chassis()
self.node.create_reader("/control", Control_Command,
self.controlcallback)
self.writer_pos = self.node.create_writer("/geek/uwb/localization",
pos)
self.writer_chassis = self.node.create_writer("/chassis", Chassis)
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
self.pos.x = 0.5
self.pos.y = 2.4
self.pos.z = 0
self.pos.yaw = self.yaw
while True:
time.sleep(0.05)
self.writer_chassis.write(self.chassis)
self.writer_pos.write(self.pos)
if self.is_sigint_up:
print("Exit")
self.is_sigint_up = False
sys.exit()
def __init__(self, node):
self.node = node
self.alpha = 0.01
self.maxTimes = 100
self.x = np.array([0.725, 0.7]);
# base station matrix
self.base_x = np.array([3.3, 3.3, 5.5, 5.5])
self.base_y = np.array([1.1, 3.38, 1.1, 3.38])
self.xi = np.array([])
self.yi = np.array([])
# distance
self.di = np.array([])
#self.di = np.array([8, 9.303, 0, 4.75])
self.localization = localization()
self.pos = pos()
self.vel_head = 0
self.yaw = 0
self.direction = 0
self.node.create_reader("/geek/gyro", Gyro, self.gyrocallback)
self.node.create_reader("/geek/uwb/pose", TagFrame, self.tagcallback)
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.writer = self.node.create_writer("/geek/uwb/localization", pos)
self.fuseflag = 0
self.v_x = 0
self.head_x = 1
self.head_y = 0
self.v_y = 0
self.direction_lag = 0
self.x_tag = 0
self.y_tag = 0
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
while True:
time.sleep(0.05)
if self.is_sigint_up:
print("Exit")
self.is_sigint_up = False
sys.exit()
def __init__(self, node):
self.node = node
self.alpha = 0.001
self.maxTimes = 100
self.x = np.array([0.725, 0.7]);
# base station matrix
self.base_x = np.array([0, 0, 2.49, 2.49])
self.base_y = np.array([3.66, 0 ,3.66, 0])
self.xi = np.array([])
self.yi = np.array([])
# distance
self.di = np.array([])
#self.di = np.array([8, 9.303, 0, 4.75])
self.localization = localization()
self.pos = pos()
self.node.create_reader("/geek/uwb/pose", TagFrame, self.tagcallback)
self.writer = self.node.create_writer("/geek/uwb/localization", pos)