Exemple #1
0
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
Exemple #2
0
#! /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!')
Exemple #3
0
#!/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()