tutorial = MoveGroupPythonIntefaceTutorial()
tutorial.go_to_initial_position()
tutorial.open_gripper()
while not rospy.is_shutdown():
velArr = [Vector3(0, 0, 0), Vector3(0, 0, 0)]
# print(ai.detectProximity())
# if ai.detectProximity():
# ai.setDistance(0.25)
# print(ai.lydar[0])
# else:
# velArr = ai.fastAdvance()
if ai.checkFrame():
# print("Markers encontrados: ", ai.markers)
if 1 not in stateMachine.values():
print("Fiquei entediado")
stateMachine["Vagando"] = 1
if stateMachine["Parado"]:
velArr = [Vector3(0, 0, 0), Vector3(0, 0, 0)]
else:
# print("nao estou parado")
if stateMachine["Vagando"]:
# print("estou vagando")
streetPoint = ai.followRoad()
if streetPoint is not None:
velArr = ai.alignToTarget(streetPoint)
stateMachine["AvancandoEstrada"] = 1
