def move():
#what to do if shut down (e.g. ctrl + C or failure)
trayectoria = get_moving()
robot = get_robot()
rospy.on_shutdown(trayectoria.shutdown)
ida = True
punto_guardian = 0
while not rospy.is_shutdown():
if trayectoria.goal_bool:
for i in range(1):
cont = 1
robot.wait(5)
#success = trayectoria.move_base.wait_for_result(rospy.Duration(cont))
#Verificar si esta cerca y en linea recta, para que se ponga otro goal y se avance "manual"
diff_x,diff_y,diff_ang = trayectoria.checkProximityGoal()
#if diff_x < 0.2 and diff_y < 0.2:
# t1 = Thread(target=robot.moveManualRobot(diff_x,diff_y,diff_ang)) #ang_pan_tilt[0],ang_pan_tilt[1]
# t1.start()
# break
#if trayectoria.interrumpir:
# trayectoria.interrumpir = False
# break
#Comentado porque caminos envia constantemente puntos y se interrumpe mucho.
#Ver si hay alguna forma de que caminos no envie tanto
#Por el momento llega hasta el goal para despues buscar otro goal
#No se si para en algun momento de tratar de detectar a la persona
#if success:
# print "break"
# break
#if not success:
# trayectoria.move_base.cancel_goal()
# rospy.loginfo("The base failed to reach the desired pose")
#else:
# # We made it!
# state = trayectoria.move_base.get_state()
# if state == GoalStatus.SUCCEEDED:
# rospy.loginfo("Hooray, reached the desired pose")
trayectoria.move_base.cancel_goal()
trayectoria.goal_bool = False
else:
punto_guardian = agregarGoal(punto_guardian,trayectoria,ida)
if punto_guardian == (len(trayectoria.poses_guardian) - 1):
ida = False
elif punto_guardian == 0:
ida = True
#! /usr/bin/python
from pyturtlebot import get_robot
from math import radians
robot = get_robot()
# You can print messages to the screen with say
robot.say('Robot is ready!')
robot.wait(1.0) # wait init
for x in range(0,4):
robot.move_distance(1, 0.25)
robot.turn_angle(radians(90), radians(30))
robot.say('Robot is finished!')
#!/usr/bin/env python
import rospy
import pyturtlebot
import numpy as np
import math
robot = pyturtlebot.get_robot()
#robot.turn_angle(radians(90))
robot.move_distance(0.1)
robot.turn_angle(math.radians(78), math.radians(78))
robot.wait(0.6)
robot.move_distance(0.1)
r = rospy.Rate(100)
while not rospy.is_shutdown():
r.sleep()
