def draw_docking_station(ax, goal_object='station'):
"""
:param ax:
:param goal_object
"""
obj_tform = Transform.scale(20) @ Transform.translate(-0.85, 0)
if goal_object == 'station':
obj_points = Point.from_list([(0, 1, 1), (2, 1, 1), (2, 0.5, 1),
(0.5, 0.5, 1), (0.5, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, -1, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
ax.add_patch(
plt.Polygon(obj_points,
facecolor=colors.to_rgba([0, 0.5, 0.5], alpha=0.5),
edgecolor=[0, 0.5, 0.5],
linewidth=1.5))
elif goal_object == 'coloured_station':
face_colours = [[0.839, 0.153, 0.157], [1.000, 0.895, 0.201],
[0.173, 0.627, 0.173]]
obj_points = Point.from_list([(0, 1, 1), (0, 0.5, 1), (2, 0.5, 1),
(2, 1, 1), (0, -1, 1), (0, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, 0.5, 1),
(0, -0.5, 1), (0.5, -0.5, 1),
(0.5, 0.5, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
arm1_points = obj_points[:4]
arm2_points = obj_points[4:8]
back_points = obj_points[8:]
ax.add_patch(
plt.Polygon(back_points,
facecolor=colors.to_rgba(face_colours[1], alpha=0.5),
edgecolor=face_colours[1],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm1_points,
facecolor=colors.to_rgba(face_colours[0], alpha=0.5),
edgecolor=face_colours[0],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm2_points,
facecolor=colors.to_rgba(face_colours[2], alpha=0.5),
edgecolor=face_colours[2],
linewidth=1.5))
else:
raise ValueError("Invalid value for goal_object")
class MarxbotPatch:
_points = Point.from_list(
[Point.ORIGIN, [1, -3, 1], [6, 0, 1], [1, 3, 1]])
def __init__(self, ax):
points = self._points.to_euclidean().T
self.circle = plt.Circle(points[0],
radius,
facecolor=colors.to_rgba(colour,
alpha=0.5),
edgecolor=colour,
linewidth=1.5,
label=label)
self.arrow = plt.Polygon(points[1:],
facecolor='none',
edgecolor=colour)
ax.add_patch(self.circle)
ax.add_patch(self.arrow)
if show_range:
self.range_circle = plt.Circle(points[0],
150.0,
facecolor=colors.to_rgba(
colour, alpha=0.1),
edgecolor=colour,
linewidth=0.5,
label='sensors range')
ax.add_patch(self.range_circle)
def update(self, position, angle):
tform = Transform.pose_transform(position, angle)
points = self._points.transformed(tform).to_euclidean().T
self.circle.center = points[0]
self.arrow.xy = points[1:]
if show_range:
self.range_circle.center = points[0]
