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')