def polyline(Nbpoints, *args):
n_arg = len(args)
N = floor(Nbpoints / (n_arg - 1))
t = np.linspace(0, 1, N)
X1 = args[0].x * (1 - t) + args[1].x * t
Y1 = args[0].y * (1 - t) + args[1].y * t
points = [Point(X1[i], Y1[i]) for i in range(N)]
for i in range(1, n_arg - 1):
print(i)
X1 = args[i].x * (1 - t) + args[i + 1].x * t
Y1 = args[i].y * (1 - t) + args[i + 1].y * t
points += [Point(X1[i], Y1[i]) for i in range(N)]
return Path(points, 0.15, 0.9, p.SPEED_MAX)
def add_precise_path(self, distance, forward=True):
self.forward = forward
self.move = Trajectory.PRECISE_MOVE
self.distance = distance
self.is_stopped = False
self.target = Point(self.robot.x + distance * cos(self.robot.theta),
self.robot.y + distance * sin(self.robot.theta))
def decelerate_to_low_speed(self, loop=False):
look_ahead_distance = p.L0 + p.k * abs(self.robot.speed)
# Test : look_ahead_distance égale à la distance de freinage à vitesse max
# t_stop = current_robot.speed / p.MAX_ACCEL
# dist_foresee = (current_robot.speed*t_stop-0.5*p.MAX_ACCEL*t_stop**2)
# look_ahead_distance = max(self.look_min,dist_foresee)
# print("L = {}, speed = {}".format(look_ahead_distance,current_robot.speed))
p_robot = Point(self.robot.x, self.robot.y)
theta = self.robot.theta
if not loop:
closest_point_index, p_goal_index, p_goal = self.path.find_goal_point(
p_robot, look_ahead_distance)
else:
closest_point_index, p_goal_index, p_goal = self.path.find_goal_point_loop(
p_robot, look_ahead_distance)
x_body = -sin(theta) * (p_goal.x - p_robot.x) + cos(theta) * (
p_goal.y - p_robot.y)
# print(p_goal.x, p_goal.y)
# dist_to_end = dist(p_robot, self.path.points[-1])
# dist_to_end = dist(p_robot, self.path.points[-1])
# dist_to_end = self.path.dists[-1]-self.path.dists[closest_point_index]
# print("closest_index : ", closest_point_index)
# print("goal_point_index : ", p_goal_index)
# print("Goal : ({}, {})".format(p_goal.x, p_goal.y))
# if p_goal == self.path.points[-1] :
# speed_cons = max(0, self.previous_cons- p.MAX_ACCEL*p.NAVIGATOR_TIME_PERIOD)
# else:
# speed_cons = min(p.SPEED_MAX, self.previous_cons + p.MAX_ACCEL*p.NAVIGATOR_TIME_PERIOD)
speed_cons = max(
p.SPEED_MAX_DECER,
abs(self.robot.speed) - p.MAX_ACCEL * p.NAVIGATOR_TIME_PERIOD)
omega_cons = ((2 * abs(speed_cons)) /
(look_ahead_distance**2)) * x_body
#print("Closest : {}, total : {}".format(closest_point, self.path.length))
if speed_cons <= p.SPEED_MAX_DECER or dist(p_robot,
self.path.points[-1]) < 200:
self.move_set = MoveSet.PATH_FINAL
print("-------------------FINAL----------------------")
if abs(omega_cons) > 4:
self.move_set = Trajectory.ABORTING
omega_cons = 0
speed_cons = 0
#print("speed : {}, L : {}, x : {}, omega : {}".format(speed_cons, look_ahead_distance, x_body, omega_cons))
return omega_cons, speed_cons
return omega_cons, speed_cons
def line(Nbpoints, A, B):
t = np.linspace(0, 1, Nbpoints)
X1 = A.x * (1 - t) + B.x * t
Y1 = A.y * (1 - t) + B.y * t
path = Path([Point(X1[i], Y1[i]) for i in range(Nbpoints)], 0.15, 0.9,
p.SPEED_MAX)
return path
def add_forwarding(self, distance, forward=True):
print("On forwarde")
self.forward = forward
self.move = Trajectory.FORWARDING
self.move_set = MoveSet.FORWARDING_BEGIN
self.distance = distance
self.is_stopped = False
self.target = Point(self.robot.x + distance * cos(self.robot.theta),
self.robot.y + distance * sin(self.robot.theta))
def parametric_curve(Nbpoints, x_equation, y_equation):
"""
Param t between 0 and 1.
:param Nbpoints: int, nb of points of the path
:param x_equation: a function f(t) for t in [0,1]
:param y_equation: a function f(t) for t in [0,1]
:return: path, class Path
"""
t = np.linspace(0, 1, Nbpoints)
X = [x_equation(t[i]) for i in range(Nbpoints)]
Y = [y_equation(t[i]) for i in range(Nbpoints)]
path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)])
return X, Y
def circle(Nbpoints, C, r):
"""
:param Nbpoints: int, nb of points of the path
:param C: cennter of the circle
:param A: departure point of the path, element of the circle. ||C-A|| = R
:return: path, class Path
"""
x_c, y_c = C.x, C.y
t = np.linspace(0, 1, Nbpoints)
X = r * np.cos(2 * pi * t) + x_c
Y = r * np.sin(2 * pi * t) + y_c
path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)], 0.15, 0.9,
p.SPEED_MAX)
return path
def lemniscate(Nbpoints, a, b):
t = np.linspace(0, 1, Nbpoints)
X = a * np.sin(2 * np.pi * (t - 0.25)) + a
Y = b * np.cos(2 * np.pi * (t - 0.25)) * np.sin(2 * np.pi * (t - 0.25))
path = Path([Point(X[i], Y[i]) for i in range(Nbpoints)])
return path