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