Nav2 Navigation Stack - rolling  main
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) -> None:
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 == y1 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(
93  'Line has zero length (All 4 points have same coordinates)'
94  )
95 
96  def isValid(self) -> bool:
97  """Check if line is valid."""
98  return self.valid_valid_
99 
100  def advance(self) -> None:
101  """Advance to the next point in the line."""
102  if self.x1_x1_ > self.x0_x0_:
103  if self.x_x_ < self.x1_x1_:
104  self.x_x_ = round(
105  self.clampclamp(self.x_x_ + self.step_size_step_size_, self.x0_x0_, self.x1_x1_), 5
106  )
107  self.y_y_ = round(self.m_m_ * self.x_x_ + self.b_b_, 5)
108  else:
109  self.valid_valid_ = False
110  elif self.x1_x1_ < self.x0_x0_:
111  if self.x_x_ > self.x1_x1_:
112  self.x_x_ = round(
113  self.clampclamp(self.x_x_ - self.step_size_step_size_, self.x1_x1_, self.x0_x0_), 5
114  )
115  self.y_y_ = round(self.m_m_ * self.x_x_ + self.b_b_, 5)
116  else:
117  self.valid_valid_ = False
118  else:
119  if self.y1_y1_ > self.y0_y0_:
120  if self.y_y_ < self.y1_y1_:
121  self.y_y_ = round(
122  self.clampclamp(self.y_y_ + self.step_size_step_size_, self.y0_y0_, self.y1_y1_), 5
123  )
124  else:
125  self.valid_valid_ = False
126  elif self.y1_y1_ < self.y0_y0_:
127  if self.y_y_ > self.y1_y1_:
128  self.y_y_ = round(
129  self.clampclamp(self.y_y_ - self.step_size_step_size_, self.y1_y1_, self.y0_y0_), 5
130  )
131  else:
132  self.valid_valid_ = False
133  else:
134  self.valid_valid_ = False
135 
136  def getX(self) -> float:
137  """Get the abscissa of the current point."""
138  return self.x_x_
139 
140  def getY(self) -> float:
141  """Get the ordinate of the current point."""
142  return self.y_y_
143 
144  def getX0(self) -> float:
145  """Get the abscissa of the initial point."""
146  return self.x0_x0_
147 
148  def getY0(self) -> float:
149  """Get the ordinate of the initial point."""
150  return self.y0_y0_
151 
152  def getX1(self) -> float:
153  """Get the abscissa of the final point."""
154  return self.x1_x1_
155 
156  def getY1(self) -> float:
157  """Get the ordinate of the final point."""
158  return self.y1_y1_
159 
160  def get_line_length(self) -> complex:
161  """Get the length of the line."""
162  return sqrt(pow(self.x1_x1_ - self.x0_x0_, 2) + pow(self.y1_y1_ - self.y0_y0_, 2))
163 
164  def clamp(self, n: float, min_n: float, max_n: float) -> float:
165  """
166  Clamp n to be between min_n and max_n.
167 
168  Args
169  ----
170  n (float): input value
171  min_n (float): minimum value
172  max_n (float): maximum value
173 
174  Returns
175  -------
176  n (float): input value clamped between given min and max
177 
178  """
179  if n < min_n:
180  return min_n
181  elif n > max_n:
182  return max_n
183  else:
184  return n
float clamp(self, float n, float min_n, float max_n)
None __init__(self, float x0, float y0, float x1, float y1, float step_size=1.0)