def __init__(self, *args, frame=None):
"""Pose initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
with suppress(Exception):
args = (args[0], Quaternion(*args[1]))
with suppress(Exception):
if isinstance(args[1], numbers.Number):
args = (args[0], Quaternion(axis=[0, 0, 1], radians=args[1]))
# Attempt default init
with suppress(IndexError, NotImplementedError, TypeError):
if type(args[1]) == Quaternion:
self.position = Point(args[0])
self.orientation = args[1]
return
# Try to initialize geometry pose
with suppress(Exception):
# Call this function with values extracted
t = Point(args[0].position)
g_quaternion = args[0].orientation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
# Try to initialize geometry transform
with suppress(Exception):
# Call this function with values extracted
t = Point(args[0].translation)
g_quaternion = args[0].rotation
q = Quaternion(g_quaternion.w, g_quaternion.x, g_quaternion.y,
g_quaternion.z)
self.__init__(t, q)
return
with suppress(Exception):
# Try to initialize with two points translation+orientation
t = args[0]
orientation_vec = Vector(args[1])
angle = (-1 if orientation_vec.y < 0 else 1) * math.acos(
(Vector(1, 0, 0) * orientation_vec) / abs(orientation_vec))
self.__init__(t, angle)
return
# None of the initializations worked
raise NotImplementedError(
f"{self.__class__} initialization not implemented for {type(args[0])}"
)
def __init__(self, *args):
"""Polygon initialization."""
# Create points out of arguments:
with suppress(NotImplementedError, IndexError, TypeError):
args = ([Point(p) for p in args[0]], 0) # Make tuple
# Try to initialize directly
with suppress(Exception):
super(Polygon, self).__init__(*args)
return
# Try to initialize from list of points
with suppress(Exception):
args = [[p.x, p.y, p.z] for p in args[0]]
super(Polygon, self).__init__(args)
return
# Try to initialize from geometry_msgs/Polygon
with suppress(Exception):
super(Polygon,
self).__init__([[p.x, p.y, p.z] for p in args[0].points])
return
# Try to initialize from two lines
with suppress(AttributeError):
points = args[1].get_points()
points.extend(reversed(args[0].get_points()))
self.__init__(points)
return
# None of the initializations worked
raise NotImplementedError(
f"Polygon initialization not implemented for {type(args[0])}")
def cut(cls, line: "Line", arc_length: float) -> Tuple["Line", "Line"]:
"""Cuts a line in two at a arc_length from its starting point.
See:
https://shapely.readthedocs.io/en/latest/manual.html?highlight=cut#linear-referencing-methods
"""
coords = list(line.coords)
if arc_length == 0:
return Line(), Line(coords)
elif arc_length == line.length:
return Line(coords), Line()
else:
assert (arc_length > 0.0
and arc_length < line.length), "Invalid arc length given."
for i, p in enumerate(coords):
pd = line.project(Point(p))
if pd == arc_length:
return (Line(coords[:i + 1]), Line(coords[i:]))
if pd > arc_length:
cp = line.interpolate(arc_length)
return (
Line(coords[:i] + [(cp.x, cp.y)], frame=line._frame),
Line([(cp.x, cp.y)] + coords[i:], frame=line._frame),
)
def get_points(self) -> List[Point]:
"""Points of polygon.
Returns:
list of points on the polygon.
"""
return [Point(x, y, z) for x, y, z in self.exterior.coords]
def test_pose_init(self):
""" Test if pose class can be initialized as expected. """
# Create from quaternion
p = Point(1, 3, 2)
o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2))
pose = Pose(p, o)
# Create from angle
pose2 = Pose(p, math.pi / 2)
self.assertEqual(pose, pose2)
g_pose = g_msgs.Pose()
g_pose.position = p.to_geometry_msg()
g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_pose), pose)
# Create from geometry_msgs/transform
g_tf = g_msgs.Transform()
g_tf.translation = p.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_tf), pose)
def get_points(self) -> List[Point]:
"""Points of line.
Returns:
list of points on the line.
Rotate the line tf.rotation around (0,0,0) and translate by tf.xyz
"""
return [Point(x, y, z) for x, y, z in self.coords]
def state_cb(self, msg: CarStateMsg):
""" Called when car state is published
Arguments:
msg (CarStateMsg): Msg published by car state node
"""
frame_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.frame.points),
frame_id="simulation",
rgba=[0, 0, 1, 0.7])
self.frame_publisher.publish(frame_marker)
if len(msg.view_cone.points):
cone_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.view_cone.points),
frame_id="simulation",
id=1)
self.view_cone_publisher.publish(cone_marker)
def test_point_init(self):
"""Test if the point class can be initialize."""
# Basic
p1 = Point(1, 3)
self.assertListEqual([p1.x, p1.y, p1.z], [1, 3, 0])
examples = [
([-1, -2], [-1, -2, 0]),
(np.array([1, 2, 4]), [1, 2, 4]),
(np.array([1, 2]), [1, 2, 0]),
(g_msgs.Point32(1, 3, 4), [1, 3, 4]),
]
for example in examples:
# Point initialization
point = Point(example[0])
self.assertListEqual([point.x, point.y, point.z], example[1])
self.assertEqual(Point(math.sqrt(2), math.sqrt(2)), Point(r=2, phi=math.pi / 4))
def test_point_func(self):
p1 = Point(1, 2, 3)
vec = Vector(2, 1, 3)
p3 = Point(3, 3, 6)
self.assertEqual(p1 + vec, p3)
self.assertEqual(p3 - vec, p1)
# The following should raise exception
with self.assertRaises(InvalidPointOperationError):
p1.rotated(0)
with self.assertRaises(InvalidPointOperationError):
p1 + p3
with self.assertRaises(InvalidPointOperationError):
p1 - p3
with self.assertRaises(InvalidPointOperationError):
3 * p3
def state_cb(self, msg: CarStateMsg):
"""Call when car state is published.
Arguments:
msg (CarStateMsg): Msg published by car state node
"""
frame_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.frame.points),
frame_id=self.param.vehicle_simulation_link.frame.simulation,
rgba=[0, 0, 1, 0.7],
)
self.frame_publisher.publish(frame_marker)
if len(msg.view_cone.points):
cone_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.view_cone.points),
frame_id=self.param.vehicle_simulation_link.frame.simulation,
id=1,
)
self.view_cone_publisher.publish(cone_marker)
def create_points(self, count=10):
points = []
for _ in range(count):
x = random.random()
y = random.random()
z = random.random()
p = Point(x, y, z)
points.append(p)
return points
def test_transformations(self):
tf = Transform(Vector(2, 2), math.pi / 2)
self.assertEqual(Vector(1, 3), tf * Vector(1, 1))
self.assertEqual(Point(1, 3), tf * Point(1, 1))
# Rotation around x-axis by 90 degrees
tf2 = Transform(Vector(0, 0),
Quaternion(axis=(1.0, 0.0, 0.0), radians=math.pi / 2))
self.assertEqual(Vector(1, 2, 3), tf2 * Vector(1, 3, -2))
# Check if line and polygon are transformed correctly
points = self.create_points()
transformed_points = [tf * p for p in points]
self.assertEqual(tf * Line(points), Line(transformed_points))
self.assertEqual(tf * Polygon(points), Polygon(transformed_points))
# Check to transform a pose
pose = Pose(Point(2, 2), math.pi)
self.assertEqual(tf * pose, Pose([0, 4], math.pi * 3 / 2))
def test_pose_funcs(self):
p = Point(1, 3, 2)
# Create from angle
pose = Pose(p, math.pi / 2)
self.assertAlmostEqual(pose.get_angle(), math.pi / 2)
g_pose = g_msgs.Pose()
g_pose.position = p.to_geometry_msg()
g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.geom_pose_almost_eq(g_pose, pose.to_geometry_msg())
# Should return 90 degrees
pose2 = Pose(Point(1, 0, 0), Quaternion(-1, 0, 0, -1).normalised)
self.assertEqual(pose2.get_angle(), math.pi / 2)
# Apply transformations
tf2 = Transform(Vector(1, 0, 0), Quaternion(1, 0, 0, 1).normalised)
# Multiply transforms
self.assertEqual(tf2 * pose, Pose(Vector(1, 4, 2), math.pi))
def test_line_init(self):
points = self.create_points()
line = Line(points)
g_line = [p.to_geometry_msg() for p in points]
self.assertEqual(Line(g_line), line)
np_array = [p.to_numpy() for p in points]
self.assertEqual(Line(np_array), line)
# List of two dimensional coords
projected_points = [Point(p.x, p.y) for p in points]
coords_2d = [[p.x, p.y] for p in points]
self.assertEqual(Line(projected_points), Line(coords_2d))
def interpolate_pose(self, *, arc_length: float) -> Pose:
"""Interpolate the pose a model travelling along this line has.
Args:
arc_length (float): Length along the line starting from the first point
Raises:
ValueError: If the arc_length is <0 or more than the length of the line.
Returns:
Corresponding pose.
"""
point = self.interpolate(arc_length)
orientation = self.interpolate_direction(arc_length=arc_length)
return Pose(Point(point), orientation)
def test_line_func(self):
points = self.create_points(10)
line = Line()
line += Line(points[:5])
line += Line(points[5:])
self.assertEqual(Line(points), line)
line = Line([x, x] for x in range(10))
l_r = Line([x + math.sqrt(2), x - math.sqrt(2)] for x in range(10))
l_p = line.parallel_offset(2, "right")
self.assertEqual(l_p, l_r, f"Lines {l_r} and {l_p} are not equal.")
tf = Transform(Point(1, 0, 2), math.pi / 2)
self.assertEqual(tf * line, Line([1 - x, x, 2] for x in range(10)))
def __init__(self, *args):
"""Line initialization."""
if len(args) == 0:
args = ([], None)
# Catch missing z coordinate by converting to point
with suppress(NotImplementedError, IndexError):
args = ([Point(arg) for arg in args[0]], None)
# Try to initialize from list of Point or geometry_msgs/Point
with suppress(NotImplementedError, AttributeError):
super(Line, self).__init__([[p.x, p.y, p.z] for p in args[0]])
return
# None of the initializations worked
raise NotImplementedError(
f"Line initialization not implemented for {type(args[0])}")
def __init__(self, *args, frame=None):
"""Polygon initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
# Create points out of arguments:
with suppress(NotImplementedError, IndexError, TypeError):
args = ([Point(p) for p in args[0]], 0) # Make tuple
# Try to initialize directly
with suppress(Exception):
super().__init__(*args)
return
# Try to initialize from list of points
with suppress(Exception):
args = [[p.x, p.y, p.z] for p in args[0]]
super().__init__(args)
return
# Try to initialize from geometry_msgs/Polygon
with suppress(Exception):
super().__init__([[p.x, p.y, p.z] for p in args[0].points])
return
# Try to initialize from two lines
with suppress(AttributeError):
points = args[1].get_points()
points.extend(reversed(args[0].get_points()))
self.__init__(points)
return
# None of the initializations worked
raise NotImplementedError(
f"Polygon initialization not implemented for {type(args[0])}")
def __init__(self, *args, frame=None):
"""Line initialization."""
# Due to recursive calling of the init function, the frame should be set
# in the first call within the recursion only.
if not hasattr(self, "_frame"):
self._frame = frame
if len(args) == 0:
args = ([], None)
# Catch missing z coordinate by converting to point
with suppress(NotImplementedError, IndexError):
args = ([Point(arg) for arg in args[0]], None)
# Try to initialize from list of Point or geometry_msgs/Point
with suppress(NotImplementedError, AttributeError):
super().__init__([[p.x, p.y, p.z] for p in args[0]])
return
# None of the initializations worked
raise NotImplementedError(
f"Line initialization not implemented for {type(args[0])}")
def test_point_extract(self):
p1 = Point(1, 3, 2)
self.assertEqual(p1.to_geometry_msg(), g_msgs.Point32(1, 3, 2))
def test_line_interpolation_func(self):
line = Line([Point(0, 0), Point(1, 1)])
# Test if line direction function works
self.assertAlmostEqual(
line.interpolate_direction(arc_length=0),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length / 2),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(line.interpolate_curvature(arc_length=0),
0,
delta=TOLERANCE)
self.assertAlmostEqual(
line.interpolate_curvature(arc_length=line.length / 4),
0,
delta=TOLERANCE)
self.assertEqual(line.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector(1, 1, 0)))
circle = Line(
reversed([
p for p in Point(-1, 0).buffer(1,
resolution=4096).exterior.coords
]))
# Test if line direction function works
self.assertAlmostEqual(circle.interpolate_direction(arc_length=0),
Vector(0, 1),
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 2),
Vector(0, -1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length),
Vector(0, 1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length * 3 / 4),
Vector(1, 0))
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 4),
Vector(-1, 0))
self.assertAlmostEqual(circle.interpolate_curvature(arc_length=0),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 2),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 4),
1,
delta=TOLERANCE)
short_line = Line([
[0, 0],
[line_module.CURVATURE_APPROX_DISTANCE * 0.5, 0],
[
line_module.CURVATURE_APPROX_DISTANCE * 0.5,
line_module.CURVATURE_APPROX_DISTANCE * 0.5,
],
])
with self.assertRaises(ValueError):
short_line.interpolate_curvature(0)
just_long_enough_line = Line(
[[0, 0], [2 * line_module.CURVATURE_APPROX_DISTANCE, 0]])
self.assertEqual(just_long_enough_line.interpolate_curvature(0), 0)
def assert_approx_equal_pose(pose1, pose2):
"""Substitute self.assertAlmostEqual because pose - pose is not implemented."""
self.assertAlmostEqual(Vector(pose1.position),
Vector(pose2.position))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
assert_approx_equal_pose(circle.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector([0, 1, 0])))
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 4),
Pose(Point(-1, 1), Vector([-1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 2),
Pose(Point(-2, 0), Vector([0, -1, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length * 3 / 4),
Pose(Point(-1, -1), Vector([1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length),
Pose(Point(0, 0), Vector([0, 1, 0])),
)
def test_line_interpolation_func(self):
line = Line([Point(0, 0), Point(1, 1)])
# Test if line direction function works
self.assertAlmostEqual(
line.interpolate_direction(arc_length=0),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length / 2),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(
line.interpolate_direction(arc_length=line.length),
Vector(r=1, phi=math.pi / 4),
delta=TOLERANCE,
)
self.assertAlmostEqual(line.interpolate_curvature(arc_length=0),
0,
delta=TOLERANCE)
self.assertAlmostEqual(
line.interpolate_curvature(arc_length=line.length / 4),
0,
delta=TOLERANCE)
self.assertEqual(line.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector(1, 1, 0)))
circle = Line(
reversed([
p for p in Point(-1, 0).buffer(1,
resolution=4096).exterior.coords
]))
# Test if line direction function works
self.assertAlmostEqual(circle.interpolate_direction(arc_length=0),
Vector(0, 1),
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 2),
Vector(0, -1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length),
Vector(0, 1),
delta=TOLERANCE,
)
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length * 3 / 4),
Vector(1, 0))
self.assertAlmostEqual(
circle.interpolate_direction(arc_length=circle.length / 4),
Vector(-1, 0))
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 2),
1,
delta=TOLERANCE)
self.assertAlmostEqual(
circle.interpolate_curvature(arc_length=circle.length / 4),
1,
delta=TOLERANCE)
def assert_approx_equal_pose(pose1, pose2):
self.assertAlmostEqual(Vector(pose1), Vector(pose2))
self.assertAlmostEqual(pose1.get_angle(),
pose2.get_angle(),
delta=math.radians(0.02))
assert_approx_equal_pose(circle.interpolate_pose(arc_length=0),
Pose(Point(0, 0), Vector([0, 1, 0])))
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 4),
Pose(Point(-1, 1), Vector([-1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length / 2),
Pose(Point(-2, 0), Vector([0, -1, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length * 3 / 4),
Pose(Point(-1, -1), Vector([1, 0, 0])),
)
assert_approx_equal_pose(
circle.interpolate_pose(arc_length=circle.length),
Pose(Point(0, 0), Vector([0, 1, 0])),
)
def test_polygon_func(self):
"""Test polygon functions."""
poly = Polygon(
[Point(1, 0),
Point(2, 1),
Point(2, 2),
Point(-1, 0, 2)])
# Rotate by 90 degrees -> [0,1],[-1,2],[-2,2],[0,-1,2]
# Translate by 1,0,2 -> [1,1,2],[0,2,2],[-1,2,2],[1,-1,4]
tf = Transform(Point(1, 0, 2), math.pi / 2)
self.assertEqual(
tf * poly,
Polygon([
Point(1, 1, 2),
Point(0, 2, 2),
Point(-1, 2, 2),
Point(1, -1, 4)
]),
)
# Test polygon eq function
poly_rev = Polygon([
Point(1, 1, 2),
Point(1, -1, 4),
Point(-1, 2, 2),
Point(0, 2, 2),
Point(1, 1, 2),
])
poly_uneq = Polygon([Point(2, 2), Point(2, 1), Point(1, 0)])
self.assertEqual(poly, poly)
self.assertEqual(tf * poly, poly_rev)
self.assertNotEqual(tf * poly, poly_uneq)