Skip to content

Commit 607019c

Browse files
afifswaidanAfif-Swaidan
authored and
Joshua Wallace
committed
Added Line Iterator (ros-navigation#3197)
* Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <[email protected]>
1 parent 80bd879 commit 607019c

File tree

3 files changed

+286
-0
lines changed

3 files changed

+286
-0
lines changed

‎nav2_simple_commander/nav2_simple_commander/line_iterator.py‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
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
71+
self.y0_ = y0
72+
self.x1_ = x1
73+
self.y1_ = y1
74+
self.x_ = x0
75+
self.y_ = y0
76+
self.step_size_ = step_size
77+
78+
if x1 != x0 and y1 != y0:
79+
self.valid_ = True
80+
self.m_ = (y1-y0)/(x1-x0)
81+
self.b_ = y1 - (self.m_*x1)
82+
elif x1 == x0 and y1 != y0:
83+
self.valid_ = True
84+
elif y1 == y1 and x1 != x0:
85+
self.valid_ = True
86+
self.m_ = (y1-y0)/(x1-x0)
87+
self.b_ = y1 - (self.m_*x1)
88+
else:
89+
self.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_
96+
97+
def advance(self):
98+
"""Advance to the next point in the line."""
99+
if self.x1_ > self.x0_:
100+
if self.x_ < self.x1_:
101+
self.x_ = round(self.clamp(
102+
self.x_ + self.step_size_, self.x0_, self.x1_), 5)
103+
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
104+
else:
105+
self.valid_ = False
106+
elif self.x1_ < self.x0_:
107+
if self.x_ > self.x1_:
108+
self.x_ = round(self.clamp(
109+
self.x_ - self.step_size_, self.x1_, self.x0_), 5)
110+
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
111+
else:
112+
self.valid_ = False
113+
else:
114+
if self.y1_ > self.y0_:
115+
if self.y_ < self.y1_:
116+
self.y_ = round(self.clamp(
117+
self.y_ + self.step_size_, self.y0_, self.y1_), 5)
118+
else:
119+
self.valid_ = False
120+
elif self.y1_ < self.y0_:
121+
if self.y_ > self.y1_:
122+
self.y_ = round(self.clamp(
123+
self.y_ - self.step_size_, self.y1_, self.y0_), 5)
124+
else:
125+
self.valid_ = False
126+
else:
127+
self.valid_ = False
128+
129+
def getX(self):
130+
"""Get the abscissa of the current point."""
131+
return self.x_
132+
133+
def getY(self):
134+
"""Get the ordinate of the current point."""
135+
return self.y_
136+
137+
def getX0(self):
138+
"""Get the abscissa of the initial point."""
139+
return self.x0_
140+
141+
def getY0(self):
142+
"""Get the ordinate of the intial point."""
143+
return self.y0_
144+
145+
def getX1(self):
146+
"""Get the abscissa of the final point."""
147+
return self.x1_
148+
149+
def getY1(self):
150+
"""Get the ordinate of the final point."""
151+
return self.y1_
152+
153+
def get_line_length(self):
154+
"""Get the length of the line."""
155+
return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.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

‎nav2_simple_commander/pytest.ini‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
[pytest]
2+
junit_family=xunit2

‎nav2_simple_commander/test/test_line_iterator.py‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
# Copyright 2022 Afif Swaidan
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import unittest
16+
from cmath import sqrt
17+
from nav2_simple_commander.line_iterator import LineIterator
18+
19+
20+
class TestLineIterator(unittest.TestCase):
21+
22+
def test_type_error(self):
23+
# Test if a type error raised when passing invalid arguements types
24+
self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1')
25+
26+
def test_value_error(self):
27+
# Test if a value error raised when passing negative or zero step_size
28+
self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2)
29+
# Test if a value error raised when passing zero length line
30+
self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1)
31+
32+
def test_get_xy(self):
33+
# Test if the initial and final coordinates are returned correctly
34+
lt = LineIterator(0, 0, 5, 5, 1)
35+
self.assertEqual(lt.getX0(), 0)
36+
self.assertEqual(lt.getY0(), 0)
37+
self.assertEqual(lt.getX1(), 5)
38+
self.assertEqual(lt.getY1(), 5)
39+
40+
def test_line_length(self):
41+
# Test if the line length is calculated correctly
42+
lt = LineIterator(0, 0, 5, 5, 1)
43+
self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2)))
44+
45+
def test_straight_line(self):
46+
# Test if the calculations are correct for y = x
47+
lt = LineIterator(0, 0, 5, 5, 1)
48+
i = 0
49+
while lt.isValid():
50+
self.assertEqual(lt.getX(), lt.getX0() + i)
51+
self.assertEqual(lt.getY(), lt.getY0() + i)
52+
lt.advance()
53+
i += 1
54+
55+
# Test if the calculations are correct for y = 2x (positive slope)
56+
lt = LineIterator(0, 0, 5, 10, 1)
57+
i = 0
58+
while lt.isValid():
59+
self.assertEqual(lt.getX(), lt.getX0() + i)
60+
self.assertEqual(lt.getY(), lt.getY0() + (i*2))
61+
lt.advance()
62+
i += 1
63+
64+
# Test if the calculations are correct for y = -2x (negative slope)
65+
lt = LineIterator(0, 0, 5, -10, 1)
66+
i = 0
67+
while lt.isValid():
68+
self.assertEqual(lt.getX(), lt.getX0() + i)
69+
self.assertEqual(lt.getY(), lt.getY0() + (-i*2))
70+
lt.advance()
71+
i += 1
72+
73+
def test_hor_line(self):
74+
# Test if the calculations are correct for y = 0x+b (horizontal line)
75+
lt = LineIterator(0, 10, 5, 10, 1)
76+
i = 0
77+
while lt.isValid():
78+
self.assertEqual(lt.getX(), lt.getX0() + i)
79+
self.assertEqual(lt.getY(), lt.getY0())
80+
lt.advance()
81+
i += 1
82+
83+
def test_ver_line(self):
84+
# Test if the calculations are correct for x = n (vertical line)
85+
lt = LineIterator(5, 0, 5, 10, 1)
86+
i = 0
87+
while lt.isValid():
88+
self.assertEqual(lt.getX(), lt.getX0())
89+
self.assertEqual(lt.getY(), lt.getY0() + i)
90+
lt.advance()
91+
i += 1
92+
93+
def test_clamp(self):
94+
# Test if the increments are clamped to avoid crossing the final points
95+
# when step_size is large with respect to line length
96+
lt = LineIterator(0, 0, 5, 5, 10)
97+
self.assertEqual(lt.getX(), 0)
98+
self.assertEqual(lt.getY(), 0)
99+
lt.advance()
100+
while lt.isValid():
101+
self.assertEqual(lt.getX(), 5)
102+
self.assertEqual(lt.getY(), 5)
103+
lt.advance()
104+
105+
106+
if __name__ == '__main__':
107+
unittest.main()

0 commit comments

Comments
 (0)