def on_move_curve(self, packet):
self.layer.robot_controller.send_packet(packets.GotoStarted())
p1 = self.get_pose()
for p2 in packet.points:
p2.angle = tools.angle_between(p1.x, p1.y, p2.x, p2.y)
p1 = p2
if packet.angle is not None:
p1.angle = packet.angle
self.current_goto_packet = packet
self.stop_reason = REASON_DESTINATION_REACHED
self.process()
def robot_arc(self, center_x, center_y, radius, angle, direction):
pose = self.get_pose()
ra = tools.angle_between(center_x, center_y, pose.x, pose.y)
da = angle + math.copysign(math.pi / 2.0, angle - ra)
if direction < 0:
da += math.pi
rotate_animation = self.create_rotation_animation(da)
pos_animation = self.create_arc_animation(center_x, center_y, radius, angle)
rotate_animation.setDuration(pos_animation.duration() * 5)
pos_animation.setDuration(pos_animation.duration() * 5)
self.move_animation.clear()
self.move_animation.addAnimation(rotate_animation)
self.move_animation.addAnimation(pos_animation)
self.move_animation.start()
def create_path(self):
cost, path = self.event_loop.map.route(self.robot.pose,
self.destination)
if len(path) == 0 or self.offset == 0:
return path
else:
p = self.robot.pose if len(path) == 1 else path[-2]
a = tools.angle_between(p.x, p.y, self.destination.x,
self.destination.y)
dist = tools.distance(p.x, p.y, self.destination.x,
self.destination.y)
dist += self.offset
last_pose = path[-1]
last_pose.x = p.x + math.cos(a) * dist
last_pose.y = p.y + math.sin(a) * dist
return path
def create_arc_animation(self, center_x, center_y, radius, angle):
r = radius * 1000.0
cx = center_y * 1000.0
cy = center_x * 1000.0
ca = tools.angle_between(cx, cy, self.item.pos().x(), self.item.pos().y())
da = self.convert_angle(angle) - ca
duration = abs(r * da)
pos_animation = QPropertyAnimation()
pos_animation.setTargetObject(self)
pos_animation.setPropertyName("position")
pos_animation.setDuration(duration)
pos_animation.setStartValue(self.item.pos())
for i in range(10):
step = float(i + 1) / 10.0
x = cx + math.cos(ca + step * da) * r
y = cy + math.sin(ca + step * da) * r
pos_animation.setKeyValueAt(step, QPointF(x, y))
return pos_animation
