Nav2 Navigation Stack - jazzy  jazzy
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, y0, x1, y1, step_size=1.0):
35  """
36  Initialize the LineIterator.
37 
38  Args
39  ----
40  x0 (float): Abscissa of the initial point
41  y0 (float): Ordinate of the initial point
42  x1 (float): Abscissa of the final point
43  y1 (float): Ordinate of the final point
44  step_size (float): Optional, Increments' resolution, defaults to 1
45 
46  Raises
47  ------
48  TypeError: When one (or more) of the inputs is not a number
49  ValueError: When step_size is not a positive number
50 
51  """
52  if type(x0) not in [int, float]:
53  raise TypeError('x0 must be a number (int or float)')
54 
55  if type(y0) not in [int, float]:
56  raise TypeError('y0 must be a number (int or float)')
57 
58  if type(x1) not in [int, float]:
59  raise TypeError('x1 must be a number (int or float)')
60 
61  if type(y1) not in [int, float]:
62  raise TypeError('y1 must be a number (int or float)')
63 
64  if type(step_size) not in [int, float]:
65  raise TypeError('step_size must be a number (int or float)')
66 
67  if step_size <= 0:
68  raise ValueError('step_size must be a positive number')
69 
70  self.x0_x0_ = x0
71  self.y0_y0_ = y0
72  self.x1_x1_ = x1
73  self.y1_y1_ = y1
74  self.x_x_ = x0
75  self.y_y_ = y0
76  self.step_size_step_size_ = step_size
77 
78  if x1 != x0 and y1 != y0:
79  self.valid_valid_ = True
80  self.m_m_ = (y1 - y0) / (x1 - x0)
81  self.b_b_ = y1 - (self.m_m_ * x1)
82  elif x1 == x0 and y1 != y0:
83  self.valid_valid_ = True
84  elif y1 == y1 and x1 != x0:
85  self.valid_valid_ = True
86  self.m_m_ = (y1 - y0) / (x1 - x0)
87  self.b_b_ = y1 - (self.m_m_ * x1)
88  else:
89  self.valid_valid_ = False
90  raise ValueError(
91  'Line has zero length (All 4 points have same coordinates)'
92  )
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 intial 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, min_n, max_n):
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 __init__(self, x0, y0, x1, y1, step_size=1.0)