def __init__(self, x, y, robot_state, robot_param, vel = 1.0, length_path_end=5.0) :
# Class Variables
self._mutex = Lock()
# Arguments
self.x = x
self.y = y
self.wheelbase = robot_param.wheelbase
self.offset = robot_param.length_offset
self.min_length_goal_reached = length_path_end
self.robot_state = robot_state
# Calculate related path element/stuff/foo
self.dx, self.dy = self._calc_dot()
self.ddx, self.ddy = self._calc_ddot()
self.yaw = self._calc_yaw(self.dx, self.dy, vel)
self.curve = self._calc_curve(self.dx, self.ddx, self.dy, self.ddy)
self.steer = self._calc_steer(self.curve, vel)
self.x_offset, self.y_offset = self._calc_offset_path(self.x, self.y, self.yaw)
self.arclength, self.ds, self.ds_mean = self._calc_arclength(self.dx, self.dy)
self.steerRate = self._calc_steerRate(self.steer, self.ds, vel)
# Set Goal
self.lateral_error = np.full(shape=(len(self.arclength),), fill_value=np.NAN)
self.goal = xyYaw_to_matrix(self.x_offset[-1], self.y_offset[-1], self.yaw[-1])
self.goalInv = inv(self.goal)
self.index_nn = -1
self.index_nn_lh = -1
self.meters_around_last_nnidx = 0.3 * vel # Multiplication with the desired/mean velocity for normalization
self.drivenPath = DrivenPath( )
self.referencePath = None
self.predictionPath__dlqt_mpc = None
def _get_errorMatrix(self, desired, l10nOffset, in_robotframe=False) :
xo, yo, yaw = l10nOffset[0], l10nOffset[1], l10nOffset[2]
actual = xyYaw_to_matrix(xo, yo, yaw)
if in_robotframe == False:
diff_tf = inv(desired) @ actual
else: # in Robot-Frame
diff_tf = inv(actual) @ desired
pass
return diff_tf
def _l10n_discontinous(self, yawRate):
xyYaw_now = self.get_xyYaw()
tfMatrix_now = mytf.xyYaw_to_matrix(xyYaw_now)
if self.xyYaw_update is None:
self.xyYaw_update = xyYaw_now
if mytf.getLength2D(xyYaw_now,
self.xyYaw_update) > self.l10n_update_distance:
self.xyYaw_update = xyYaw_now
dx = np.random.normal(0.0, self.var_xy)
dy = np.random.normal(0.0, self.var_xy)
dYaw = np.random.normal(0.0, self.var_yaw)
dYaw = mytf.getYawRightRange(dYaw)
self.dtfMatrix = mytf.xyYaw_to_matrix(dx, dy, dYaw)
map_2_l10n = tfMatrix_now @ self.dtfMatrix
lx, ly, lYaw = map_2_l10n[0, 3], map_2_l10n[1, 3], mytf.matrix_to_yaw(
map_2_l10n)
self._set_l10n_pose(lx, ly, lYaw)
def _get_indexNearestPosition_byCircle(self, l10nOffset, lookahead_distance):
idx_around_last_nn = int( self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 )
# Define Start-Idx for the Loop
start_index = self.index_nn_lh - idx_around_last_nn
if start_index < 0 :
start_index = 0
# Define End-Idx for the Loop
if self.index_nn_lh < 0: # self.index_lookahead is not inited
end_index = len(self.x)
else:
end_index = self.index_nn_lh + idx_around_last_nn
if end_index > len(self.x) :
end_index = len(self.x)
# GET DESIRED-POSE
dist_to_lookahead = float("inf")
path_desired_idx = self.index_nn_lh
actual_tfMatrix = xyYaw_to_matrix(l10nOffset[0], l10nOffset[1], l10nOffset[2])
for i in range(start_index, end_index) :
iPath_tfMat = xyYaw_to_matrix(self.x_offset[i], self.y_offset[i], self.yaw[i])
dist_offset2path = getLength2DSign(actual_tfMatrix, iPath_tfMat)
if dist_offset2path < 0.0 :
continue
else :
eDist = dist_offset2path - lookahead_distance
if eDist <= 0.0 :
continue
elif eDist < dist_to_lookahead :
dist_to_lookahead = eDist
path_desired_idx = i
if np.fabs(dist_to_lookahead) < np.fabs(self.ds_mean):
break
desired_pose_matrix = xyYaw_to_matrix(self.x_offset[path_desired_idx], self.y_offset[path_desired_idx], self.yaw[path_desired_idx])
return path_desired_idx, dist_offset2path, desired_pose_matrix
def _get_indexNearestPosition(self, robot_state, look_ahead=None):
robot_state_lh = [robot_state[0], robot_state[1], robot_state[2] ]
# print("robot_state_lh:", robot_state_lh)
# sleep(11.1)
if look_ahead is not None:
# print("use look ahead")
# print("before lh:", robot_state[0] )
robot_state_lh[0] = robot_state_lh[0] + look_ahead * cos(robot_state[2])
robot_state_lh[1] = robot_state_lh[1] + look_ahead * sin(robot_state[2])
# print("after lh:", robot_state[0] )
last_nn_idx = self.index_nn_lh
else :
last_nn_idx = self.index_nn #-1
# Define Start-Index and End-Index for the Loop
start_index = last_nn_idx - int(self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 )
if start_index < 0 : start_index = 0
if last_nn_idx < 0: # at init, search in whole path
end_index = len(self.x)
else:
end_index = last_nn_idx + int( self.meters_around_last_nnidx / np.fabs(self.ds_mean)+1 )
if end_index > len(self.x) : end_index = len(self.x)
# Get index for neatest euclidean distance
dL = float('Inf')
index = 0
xo, yo, yaw = robot_state_lh[0], robot_state_lh[1], robot_state_lh[2]
# print("start_index: ", start_index)
# print("end_index: ", end_index)
# print("self.meters_around_last_nnidx: ", self.meters_around_last_nnidx)
# print("int( self.meters_around_last_nnidx / np.fabs(self.ds_mean) = ", int( self.meters_around_last_nnidx / np.fabs(self.ds_mean) +1) )
for i in range(start_index, end_index) :
dL_ = np.hypot(self.x_offset[i] - xo, self.y_offset[i] - yo)
if dL_ < dL :
index = i
dL = dL_
# xyYaw = [self.x[index], self.y[index], self.yaw[index] ]
pose_matrix = xyYaw_to_matrix(self.x_offset[index], self.y_offset[index], self.yaw[index])
return index, dL, pose_matrix
def __init__(self,
robot_parameter,
x=0.0,
y=0.0,
yaw=0.0,
steerAngle=0.0,
velocity=0.0,
var_xy=0.0,
var_yaw=0.0,
l10n_update_distance=0.0):
self._mutex = Lock()
self.look_ahead = robot_parameter.look_ahead
self.length_offset = robot_parameter.length_offset
self.var_xy = var_xy
self.var_yaw = var_yaw
self.l10n_update_distance = l10n_update_distance
self.xyYaw_update = None
self.dtfMatrix = mytf.xyYaw_to_matrix(x=0.0, y=0.0, yaw=0.0)
# POSE
self.x = x
self.y = y
self.xo = x + self.length_offset * cos(yaw)
self.yo = y + self.length_offset * sin(yaw)
self.yaw = yaw
self.yawRate = 0.0
# Localization
self.lx = x
self.ly = y
self.lYaw = yaw
self.lxo = self.xo
self.lyo = self.yo
# STEERING
self.steerAngle = steerAngle
self.steerAngleDes = 0.0
self.steerAngleRate = 0.0
self.steerAngleRateDes = 0.0
self.deltaVel = 0.0
self.deltaVelDes = 0.0
# DRIVE
self.velocity = velocity
self.accel = 0.0
def _is_behindGoal(self, robot_state, velocity):
xo, yo, yaw = robot_state[0], robot_state[1], robot_state[2]
actual = xyYaw_to_matrix(xo, yo, yaw)
goal2actual = self.goalInv @ actual
length_to_goal = np.hypot( goal2actual[0,3], goal2actual[1,3] )
if length_to_goal > self.min_length_goal_reached :
return False
else :
dx = goal2actual[0, 3]
if velocity > 0.0 and dx > 0.0:
return True
elif velocity < 0.0 and dx < 0.0:
return True
else :
False
def _get_errorMatrix(self, desired, robot_state) :
xo, yo, yaw = robot_state[0], robot_state[1], robot_state[2]
actual = xyYaw_to_matrix(xo, yo, yaw)
diff_tf = inv(desired) @ actual
return diff_tf
##################
if __name__ == "__main__":
print("MAIN for testing the class")
rp = Robot_Parameter()
rs = Robot_State(rp)
# rs.x = 3.0
# print(rsd.get_asVector() )
rsd_disc.set_pose(x=4.9, y=2.6, yaw=1.2)
print("BLA:", rsd_disc.get_asVector())
# Test Varaiance
max_ = 0.0
for a in range(1):
rand_x = np.random.normal(0.0, rsd_disc.var_xy)
rand_y = np.random.normal(0.0, rsd_disc.var_xy)
print("dist", np.hypot(rand_x, rand_y), "rand_x:", rand_x, "rand_y:",
rand_y)
dist = np.hypot(rand_x, rand_y)
if dist > max_:
max_ = dist
print("max_", max_)
print(mytf.xyYaw_to_matrix(x=1.0, y=2.0, yaw=1.4))