Nav2 Navigation Stack - lyrical  lyrical
ROS 2 Navigation Stack
line_iterator.py
1 #! /usr/bin/env python3
2 # Copyright 2021 Samsung Research America
3 # Copyright 2022 Afif Swaidan
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 
17 """
18 This is a Python3 API for a line iterator.
19 
20 It provides the ability to iterate
21 through the points of a line.
22 """
23 
24 from cmath import sqrt
25 
26 
28  """
29  LineIterator.
30 
31  LineIterator Python3 API for iterating along the points of a given line
32  """
33 
34  def __init__(self, x0: float, y0: float,
35  x1: float, y1: float,
36  step_size: float = 1.0):
37  """
38  Initialize the LineIterator.
39 
40  Args
41  ----
42  x0 (float): Abscissa of the initial point
43  y0 (float): Ordinate of the initial point
44  x1 (float): Abscissa of the final point
45  y1 (float): Ordinate of the final point
46  step_size (float): Optional, Increments' resolution, defaults to 1
47 
48  Raises
49  ------
50  TypeError: When one (or more) of the inputs is not a number
51  ValueError: When step_size is not a positive number
52 
53  """
54  if type(x0) not in [int, float]:
55  raise TypeError('x0 must be a number (int or float)')
56 
57  if type(y0) not in [int, float]:
58  raise TypeError('y0 must be a number (int or float)')
59 
60  if type(x1) not in [int, float]:
61  raise TypeError('x1 must be a number (int or float)')
62 
63  if type(y1) not in [int, float]:
64  raise TypeError('y1 must be a number (int or float)')
65 
66  if type(step_size) not in [int, float]:
67  raise TypeError('step_size must be a number (int or float)')
68 
69  if step_size <= 0:
70  raise ValueError('step_size must be a positive number')
71 
72  self.x0_x0_ = x0
73  self.y0_y0_ = y0
74  self.x1_x1_ = x1
75  self.y1_y1_ = y1
76  self.x_x_ = x0
77  self.y_y_ = y0
78  self.step_size_step_size_ = step_size
79 
80  if x1 != x0 and y1 != y0:
81  self.valid_valid_ = True
82  self.m_m_ = (y1 - y0) / (x1 - x0)
83  self.b_b_ = y1 - (self.m_m_ * x1)
84  elif x1 == x0 and y1 != y0:
85  self.valid_valid_ = True
86  elif y1 == y0 and x1 != x0:
87  self.valid_valid_ = True
88  self.m_m_ = (y1 - y0) / (x1 - x0)
89  self.b_b_ = y1 - (self.m_m_ * x1)
90  else:
91  self.valid_valid_ = False
92  raise ValueError('Line has zero length (All 4 points have same coordinates)')
93 
94  def isValid(self):
95  """Check if line is valid."""
96  return self.valid_valid_
97 
98  def advance(self):
99  """Advance to the next point in the line."""
100  if self.x1_x1_ > self.x0_x0_:
101  if self.x_x_ < self.x1_x1_:
102  self.x_x_ = round(
103  self.clampclamp(self.x_x_ + self.step_size_step_size_, self.x0_x0_, self.x1_x1_), 5
104  )
105  self.y_y_ = round(self.m_m_ * self.x_x_ + self.b_b_, 5)
106  else:
107  self.valid_valid_ = False
108  elif self.x1_x1_ < self.x0_x0_:
109  if self.x_x_ > self.x1_x1_:
110  self.x_x_ = round(
111  self.clampclamp(self.x_x_ - self.step_size_step_size_, self.x1_x1_, self.x0_x0_), 5
112  )
113  self.y_y_ = round(self.m_m_ * self.x_x_ + self.b_b_, 5)
114  else:
115  self.valid_valid_ = False
116  else:
117  if self.y1_y1_ > self.y0_y0_:
118  if self.y_y_ < self.y1_y1_:
119  self.y_y_ = round(
120  self.clampclamp(self.y_y_ + self.step_size_step_size_, self.y0_y0_, self.y1_y1_), 5
121  )
122  else:
123  self.valid_valid_ = False
124  elif self.y1_y1_ < self.y0_y0_:
125  if self.y_y_ > self.y1_y1_:
126  self.y_y_ = round(
127  self.clampclamp(self.y_y_ - self.step_size_step_size_, self.y1_y1_, self.y0_y0_), 5
128  )
129  else:
130  self.valid_valid_ = False
131  else:
132  self.valid_valid_ = False
133 
134  def getX(self):
135  """Get the abscissa of the current point."""
136  return self.x_x_
137 
138  def getY(self):
139  """Get the ordinate of the current point."""
140  return self.y_y_
141 
142  def getX0(self):
143  """Get the abscissa of the initial point."""
144  return self.x0_x0_
145 
146  def getY0(self):
147  """Get the ordinate of the initial point."""
148  return self.y0_y0_
149 
150  def getX1(self):
151  """Get the abscissa of the final point."""
152  return self.x1_x1_
153 
154  def getY1(self):
155  """Get the ordinate of the final point."""
156  return self.y1_y1_
157 
158  def get_line_length(self):
159  """Get the length of the line."""
160  return sqrt(pow(self.x1_x1_ - self.x0_x0_, 2) + pow(self.y1_y1_ - self.y0_y0_, 2))
161 
162  def clamp(self, n: float, min_n: float, max_n: float):
163  """
164  Clamp n to be between min_n and max_n.
165 
166  Args
167  ----
168  n (float): input value
169  min_n (float): minimum value
170  max_n (float): maximum value
171 
172  Returns
173  -------
174  n (float): input value clamped between given min and max
175 
176  """
177  if n < min_n:
178  return min_n
179  elif n > max_n:
180  return max_n
181  else:
182  return n
def clamp(self, float n, float min_n, float max_n)
def __init__(self, float x0, float y0, float x1, float y1, float step_size=1.0)