def test_polygon_init(self):
points = self.create_points()
poly = Polygon(points)
# Check that polygon contains points
# Shapely adds first point at last index again
self.assertListEqual(poly.get_points()[:-1], points)
# Geometry msgs
g_polygon = g_msgs.Polygon()
g_polygon.points = [p.to_geometry_msg() for p in points]
poly_g = Polygon(g_polygon)
self.assertEqual(poly, poly_g)
# Numpy
array = np.array([p.to_numpy() for p in points])
poly_np = Polygon(array)
self.assertEqual(poly, poly_np)
# Check if polygon can be created correctly from array of two dimensional points
points_2d = [[2, 1], [3, 2], [34, 5], [3242, 1]]
points_3d = [[*p, 0] for p in points_2d]
self.assertEqual(Polygon(points_2d), Polygon(points_3d))
# Two lines
points_left = points
points_right = [(p + Vector(1, 0)) for p in points]
all_points = points_right.copy()
all_points.extend(reversed(points_left)) # Left side in reverse
poly1 = Polygon(Line(points_left), Line(points_right))
poly2 = Polygon(all_points)
self.assertEqual(poly1, poly2)
def _to_ros_msg(self):
msg = m.Polygon()
for x, y, z in self.value:
p2 = m.Point32()
p2.x = x
p2.y = y
p2.z = z
msg.points.append(p2)
return msg
def generateTransparentObstacleMarkers(self):
poly = msg.Polygon()
try:
transparent_obstacles = [e for e in self.getEntities() if isinstance(e, TranspanrentObstacle)]
for to in transparent_obstacles:
poly.points.extend([msg.Point32(p[0], p[1], 0.0) for p in to.getSegment().coords])
except (AttributeError, IndexError, ValueError):
rospy.loginfo("Encountered problem getting transparent obstacles. Skipping...")
return poly
def to_geometry_msg(self):
"""To ROS geometry_msg.
Returns:
This polygon as a geometry_msgs.Polygon.
"""
msg = geometry_msgs.Polygon()
msg.points = [
geometry_msgs.Point32(*p.to_numpy()) for p in self.get_points()
]
return msg
def test_polygon_extract(self):
""" Test if the polygon methods extraction functions work."""
points = self.create_points()
poly = Polygon(points)
g_polygon = g_msgs.Polygon()
g_polygon.points = [p.to_geometry_msg() for p in points]
# append first point at the end to match behaviour of Polygon
g_polygon.points.append(points[0].to_geometry_msg())
array = np.array([p.to_numpy() for p in points])
self.assertEqual(poly.to_geometry_msg(), g_polygon)
self.assertEqual(poly.to_numpy().all(), array.all())
def numpy_to_polygon(polygon):
points = []
for point in polygon:
points.append(numpy_to_point(point))
return geometry_msgs.Polygon(points=points)
def numpy_to_polygon(polygon):
points = numpy_to_points(polygon)
return geometry_msgs.Polygon(points=points)
def polygon_to_ros(p):
return gm.Polygon(points=p.points)
def test_make_topic_strings_with_array_non_builtin(self):
strings = ez_model.make_topic_strings(geo_msgs.Polygon(), '/polygon')
self.assertEqual(len(strings), 3)
self.assertEqual(strings[0], '/polygon/points[0]/x')
self.assertEqual(strings[1], '/polygon/points[0]/y')
self.assertEqual(strings[2], '/polygon/points[0]/z')
