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