Esempio n. 1
0
 def __init__(self, tc=None):
     self.shutdown_requested = False
     self.pseudo_states = {}
     server_node = rospy.get_param("~server", "/turtlesim_tasks")
     default_period = rospy.get_param("~period", 0.2)
     if tc:
         self.tc = tc
     else:
         self.tc = TaskClient(server_node, default_period)
Esempio n. 2
0
 def __init__(self,tc=None):
     self.shutdown_requested = False
     self.pseudo_states={}
     self.sm = None
     server_node = rospy.get_param("~server","/turtlesim_tasks")
     default_period = rospy.get_param("~period",0.2)
     if tc:
         self.tc = tc
     else:
         self.tc = TaskClient(server_node,default_period)
     # self.tc.verbose = 2
     self.complete_srv = rospy.Service(self.tc.server_node + "/complete_mission", Empty, self.shutdown)
Esempio n. 3
0
    def __init__(self, tc=None, new_outcomes=[], period=0.2):
        self.shutdown_requested = False
        self.pseudo_states = {}
        server_node = rospy.get_param(
            "~server",
            "/turtlesim_tasks")  # FIXME: why turtlesim_tasks appear here ?
        default_period = rospy.get_param("~period", period)
        if tc:
            self.tc = tc
        else:
            self.tc = TaskClient(server_node, default_period)
        # self.tc.verbose = 2

        # Create list of default outcomes
        # ----------------------------------
        default_outcomes = [
            'TASK_COMPLETED', 'TASK_INTERRUPTED', 'TASK_FAILED',
            'TASK_TIMEOUT', 'MISSION_COMPLETED'
        ]
        for outcome in new_outcomes:
            default_outcomes.append(outcome)
        self.default_outcomes = default_outcomes
Esempio n. 4
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_uavsim')
import rospy
from math import *
from task_manager_lib.TaskClient import *
import traceback

rospy.init_node('task_client', anonymous=True)
server_node = rospy.get_param("~server", "/quad1_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[0., 0., 0.5, 0.0], [0., 2., 0.5, 1.57], [2., 0., 0.5, 1.57],
      [0., -2., 0.5, 3.14], [-2., 0., 0.5, -1.57], [0., 0., 0.5, 0.0]]

try:
    tc.SetStatusSync(status=0)
    tc.SetMotor(on=True)
    tc.TakeOff()
    tc.SetStatusSync(status=1)
    tc.WaitForStatusSync(partner="partner2", status=4)
    for i, p in enumerate(wp):
        tc.GoTo(goal_x=p[0], goal_y=p[1], goal_z=p[2])
        tc.SetHeading(goal_heading=p[3])
        tc.SetStatusSync(status=i + 2)

    tc.SetStatusSync(status=0)
    tc.Land()
    tc.SetMotor(on=False)
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

while True:

    # Start the wait for face task in the background
    w4face = tc.WaitForFace(foreground=False)
    # Prepare a condition so that the following gets executed only until the
    # Region of Interest is found
    tc.addCondition(ConditionIsCompleted("Face detector", tc, w4face))
    try:
        tc.Wander(max_linear_speed=0.2)
        # Clear the conditions if we reach this point
        tc.clearConditions()
    except TaskConditionException, e:
        tc.Wait(duration=3.)
        tc.StareAtFace(relative=True)
        tc.Wait(duration=3.)
        tc.SetHeading(target=45, relative=True, max_angular_velocity=0.2)
        tc.Wait(duration=3.)
        rospy.loginfo("Path following interrupted on condition: %s" % \
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest('task_manager_turtlesim')
import rospy
from math import *
from std_msgs.msg import Header, Float32
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/turtlesim_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[1., 9., pi / 2, 0, 0, 255], [9., 9., 0., 0, 255, 255],
      [9., 1., -pi / 2, 0, 255, 0], [1., 1., -pi, 255, 255, 0]]

while True:
    h = Header()
    h.frame_id = "Hello"
    tc.Clear(argv=h)
    f = Float32(data=1.0)
    tc.Clear(argv=f)

    tc.Wait(duration=1.)
    tc.SetPen(on=False)
    tc.GoTo(goal_x=1.0, goal_y=1.0)

    for p in wp:
        tc.Wait(duration=0.2)
Esempio n. 7
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_turtlesim_sync')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client1')
server_node = rospy.get_param("~server", "/turtlesim_tasks1")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

tc.SetStatusSync(status=0)
tc.GoTo(goal_x=1.0, goal_y=1.0)
tc.SetStatusSync(status=1)
tc.WaitForStatusSync(partner="partner2", status=1)
tc.WaitForStatusSync(partner="partner3", status=1)
tc.GoTo(goal_x=5.0, goal_y=5.0)
tc.SetStatusSync(status=0)

rospy.loginfo("Mission1 completed")
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

scale=2.0
vel=0.5

tc.WaitForAuto()
try:
    tc.Wander()

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))

if not rospy.core.is_shutdown():
    tc.SetManual()


rospy.loginfo("Mission completed")
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('sim_tasks')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

tc.SetPTZ(pan=1.57,tilt=0.0)
tc.AlignWithShore(angle=-1.57, ang_velocity=1.0)
# Set a distance trigger.
w4dist = tc.WaitForDistance(foreground=False,distance=10.0)
tc.addCondition(ConditionIsCompleted("Distance",tc,w4dist))
try:
    # Follow shore until the condition get triggered
    tc.FollowShoreRP(velocity=0.4, distance=10.0, side=+1, k_alpha=0.75, max_ang_vel=0.7,velocity_scaling=0.4)
except TaskConditionException, e:
    pass

# The distance triggered so we set a ROI trigger to make sure that we have some overlap
# when the run completes
w4roi = tc.WaitForROI(foreground=False,current=True,
        roi_radius=10.0, histeresis_radius=5.0)
tc.addCondition(ConditionIsCompleted("ROI detector",tc,w4roi))
try:
    # Follow shore until the new condition get triggered
Esempio n. 10
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *
rom geometry_msgs.msg import PoseStamped

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)

goal_dock_pub = rospy.Publisher('/goal_dock', PoseStamped, queue_size=1)

tc.WaitForAuto()
try:
    tc.Constant(duration=3.0,linear=-0.25)
    #get the pose close to the dock and store it 
	listener = tf.TransformListener()
	((x,y,_),_) = listener.lookupTransform("odom", "/base_link", rospy.Time(0))
	goal_dock = PoseStamped()
	goal_dock.pose.position.x = x
	goal_dock.pose.position.y = y
	goal_dock.pose.position.z = 0
	goal_dock.pose.orientation.x = 0
	goal_dock.pose.orientation.y = 0
	goal_dock.pose.orientation.z = 0
	goal_dock.pose.orientation.w = 1	
	
	goal_dock_pub.publish(goal_dock)
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_turtlesim')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/turtlesim_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

tc.Wait(duration=1.)

for error in [6, 7, 8, 9, 10]:
    try:
        tc.Fail(iterations=10, error_type=error)
    except TaskException, e:
        rospy.loginfo("Task fail with error status %d while expecting %d: %s" %
                      (e.status, error, str(e)))

rospy.loginfo("Mission completed")
Esempio n. 12
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

id=tc.TaskCheckBatt(foreground=False)
tc.addCondition(ConditionIsCompleted("Low_Battery",tc,id))
    

try:
	#tc.clearConditions()
	#tc.stopTask(id)
	
except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))
    
	 
	
if not rospy.core.is_shutdown():
    tc.SetManual()

#rospy.loginfo("Mission completed")
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest("task_manager_turtlesim")
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node("task_client")
server_node = rospy.get_param("~server", "/turtlesim_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [
    [1.0, 9.0, pi / 2, 0, 0, 255],
    [9.0, 9.0, 0.0, 0, 255, 255],
    [9.0, 1.0, -pi / 2, 0, 255, 0],
    [1.0, 1.0, -pi, 255, 255, 0],
]


tc.WaitForButton(text="go")
# Start the wait for button task in the background
w4abort = tc.WaitForButton(foreground=False, text="abort")
w4home = tc.WaitForButton(foreground=False, text="home")
# Prepare a condition so that the following gets executed only until the
# a button is pressed
tc.addCondition(ConditionIsCompleted("Abort", tc, w4abort))
tc.addCondition(ConditionIsCompleted("Home", tc, w4home))
Esempio n. 14
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('task_manager_lib')
import rospy
from task_manager_lib.TaskClient import *
import argparse

server_node="/task_server"
default_period=0.1

parser = argparse.ArgumentParser(description='Print the list of tasks running on a given server node')
parser.add_argument('--server', '-s',default=server_node,
        nargs=1, help='server node name, e.g. /task_server')
parser.add_argument('--period', '-p',default=default_period,type=float, 
        nargs=1, help='default period for new tasks')
args = parser.parse_args()
# print args
default_period=args.period
server_node=args.server[0]

rospy.init_node('task_list')
server_node = rospy.get_param("~server",server_node)
default_period = rospy.get_param("~period",default_period)
tc = TaskClient(server_node,default_period)

tc.printTaskList()


Esempio n. 15
0
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest("task_manager_test")
import rospy
from task_manager_lib.TaskClient import *


tc = TaskClient("/tasks", 0.5)
tc.verbose = True
tc.printTaskList()
tc.printTaskStatus()

print "-----------------"

param = {"task_name": "Test", "task_duration": 5.0}
tc.startTaskAndWait(param)
tc.printTaskStatus()

tc.Test(task_duration=6.0)

tc.Long(task_duration=4.0, main_task=False)
for i in range(1, 10):
    print "I'm doing something else"
    tc.printTaskStatus()
    rospy.sleep(1)
Esempio n. 16
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_uav')
import rospy
import time
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client_uav')

# uav task client
server_node_uav = rospy.get_param("~server", "/uavsim_tasks")
default_period_uav = rospy.get_param("~period", 0.2)
uav = TaskClient(server_node_uav, default_period_uav)
rospy.loginfo("Mission connected to UAV server: " + server_node_uav)

wp = [[0., 0], [2., -2.], [-2., 2.], [-2., -2.], [2., 2.]]
i = 0
try:
    uav.RequestTakeOff()
    uav.Wait(duration=3.0)
    uav.TakeOff()

    while True:

        w4landingBackOrder = uav.WaitForLandingBackOrder(foreground=False)
        w4lowBatteryLevel = uav.WaitForLowBatteryLevel(foreground=False)
        uav.addCondition(
            ConditionIsCompleted("Landing Back Order", uav,
                                 w4landingBackOrder))
Esempio n. 17
0
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph.picklez")

tc.WaitForAuto()
try:
    tc.Wait(duration=1.0)
    # Hand-made hamiltonian path
    node_seq = [0, 2, 1, 4, 3, 5, 6, 11, 7, 10, 9, 8]
    for node in node_seq:
        tc.GoTo(goal_x=g.vs[node]["x"], goal_y=g.vs[node]["y"])
    tc.Wait(duration=1.0)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))
Esempio n. 18
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

scale=2.0

tc.WaitForAuto()
try:
    #rospy.loginfo("")
    tc.PlanTo(goal_x=-scale,goal_y=-scale,goal_theta=0.0)
    rospy.loginfo("first plan to executed")
    tc.Wait(duration=1.0)
    tc.PlanTo(goal_x=-scale,goal_y=scale,goal_theta=0.0)
    tc.Wait(duration=1.0)
    tc.PlanTo(goal_x=scale,goal_y=scale,goal_theta=0.0)
    tc.Wait(duration=1.0)
    tc.PlanTo(goal_x=scale,goal_y=-scale,goal_theta=0.0)
    tc.Wait(duration=1.0)
    tc.PlanTo(goal_x=-scale,goal_y=-scale,goal_theta=0.0)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)

tc.WaitForAuto()
try:
    for angle in [0.0, pi / 2, pi, 3 * pi / 2, 0.0]:
        tc.Wait(duration=3.0)
        tc.SetHeading(target=angle)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))

if not rospy.core.is_shutdown():
    tc.SetManual()

rospy.loginfo("Mission completed")
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('sim_tasks')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)



def follow_until_stopped(shore_side):
    global tc
    try:
        # Follow shore until the condition get triggered
        tc.FollowShoreRP(velocity=0.5, distance=10.0, side=shore_side, k_alpha=0.75, max_ang_vel=0.7,velocity_scaling=0.4)
    except TaskConditionException, e:
        pass


# Now follow the island shore
tc.SetPTZ(pan=-1.57,tilt=0.2)
w4roi = tc.WaitForROI(foreground=False,wrtOrigin=False,roi_x=296871.51303, roi_y=5442696.42175, roi_radius=10.0)
tc.addCondition(ConditionIsCompleted("ROI detector",tc,w4roi))
follow_until_stopped(-1)

# Finally finish the run to home
Esempio n. 21
0
#!/usr/bin/python
# ROS specific imports
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/turtlesim_tasks")
default_period = rospy.get_param("~period",0.2)
tc = TaskClient(server_node,default_period)


wp = [ (1.,1.), (1.,9.), (9.,9.), (9.,1.), (1.,1.) ] 

Action=tc.getParameterListAction()

while True:
    tc.Wait(duration=1.)
    tc.Clear()
    tc.SetPen(on=False)
    tc.FollowPath(param_list_action=Action.Clear)
    for x,y in wp:
        tc.FollowPath(goal_x=x,goal_y=y, param_list_action=Action.Push)
    tc.FollowPath(param_list_action=Action.Execute)

    tc.Wait(duration=1.)
    tc.SetPen(on=False)
    tc.GoTo(goal_x=5.0,goal_y=5.0)
    tc.ReachAngle(target=pi/2)

rospy.loginfo("Mission completed")
roslib.load_manifest('turtlesim_logo')
import rospy
from task_manager_lib.TaskClient import *
from dynamic_reconfigure.encoding import *
import argparse
import signal
import math

server_node = ""
default_period = 0.1
rospy.init_node('task_console', disable_signals=False)
server_node = rospy.get_param("~server", server_node)
default_period = rospy.get_param("~period", default_period)

print "Node: " + str(server_node)
tc = TaskClient(server_node, default_period)


def signal_handler(signal, frame):
    global tc
    print "Killing all tasks by stopping the keep-alive pulse"
    tc.stopAllTasks()


signal.signal(signal.SIGINT, signal_handler)


def param_string(t):
    if t["type"] == "double" or t["type"] == "int":
        return "%s: %s in [%s,%s], default %s\n\t%s" % (t["name"],t["type"],\
                str(t["min"]),str(t["max"]),str(t["default"]),t["description"])
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_uavsim')
import rospy
from math import *
from task_manager_lib.TaskClient import *
from geometry_msgs.msg import Twist
import mission_constants as mc
import traceback

rospy.init_node('task_client', anonymous=True)
server_node = rospy.get_param("~server", "/quad1_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[2., 0., 1.0, False], [2., 2., 1.0, True], [3., 0., 1.0, True],
      [2., -2., 1.0, True], [1., 0., 1.0, True], [2., 0., 1.0, False]]

p2 = None


def p2_pose_cb(msg):
    global p2
    p2 = msg


# subscribe to /rcvdPoseQuad2 published by the communication node,
# instead of directly to /poseQuad2
sub = rospy.Subscriber("/quad2/pose", Twist, p2_pose_cb, queue_size=1)
#sub = rospy.Subscriber("/quad2/rcvdPose",Twist, p2_pose_cb,queue_size=1)
Esempio n. 24
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

scale=2.0
vel=0.5

tc.WaitForAuto()
try:
    tc.GoTo(goal_x=-scale,goal_y=-scale,max_velocity=vel)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=-scale,goal_y=scale,max_velocity=vel)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=scale,goal_y=scale,max_velocity=vel)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=scale,goal_y=-scale,max_velocity=vel)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=-scale,goal_y=-scale,max_velocity=vel)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))
Esempio n. 25
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_sync')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client3')
server_node = rospy.get_param("~server", "/task_server3")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

tc.SetStatusSync(status=0)
tc.WaitForStatusSync(partner="partner2", status=1)
tc.Wait(duration=3.0)
tc.SetStatusSync(status=1)
tc.WaitForStatusSync(partner="partner1", status=0)
tc.WaitForStatusSync(partner="partner2", status=0)
tc.SetStatusSync(status=0)

rospy.loginfo("Mission3 completed")
Esempio n. 26
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_turtlesim')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/turtlesim_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[1., 9., pi / 2, 0, 0, 255], [9., 9., 0., 0, 255, 255],
      [9., 1., -pi / 2, 0, 255, 0], [1., 1., -pi, 255, 255, 0]]

while True:
    tc.Wait(duration=1.)
    tc.SetPen(on=False)
    tc.GoTo(goal_x=1.0, goal_y=1.0)
    tc.Clear()

    # Start the wait for roi task in the background
    w4roi = tc.WaitForROI(foreground=False, roi_x=1., roi_y=6., roi_radius=1.0)
    # Prepare a condition so that the following gets executed only until the
    # Region of Interest is found
    tc.addCondition(ConditionIsCompleted("ROI detector", tc, w4roi))
    try:
        for p in wp:
            tc.Wait(duration=0.2)
            tc.ReachAngle(target=p[2])
Esempio n. 27
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('task_manager_uav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/uav_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[1., 9., pi / 2], [9., 9., 0.], [9., 1., -pi / 2], [1., 1., -pi]]

while True:
    tc.Wait(duration=1.)
    tc.GoTo(goal_x=1.0, goal_y=1.0)

    for p in wp:
        tc.Wait(duration=0.2)
        tc.ReachAngle(target=p[2])
        tc.GoTo(goal_x=p[0], goal_y=p[1])

    tc.Wait(duration=2.)
    tc.GoTo(goal_x=5.0, goal_y=5.0)
    tc.ReachAngle(target=pi / 2)

rospy.loginfo("Mission completed")
Esempio n. 28
0
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest('task_manager_uavsim')
import rospy
from math import *
from task_manager_lib.TaskClient import *
import traceback

rospy.init_node('task_client', anonymous=True)
server_node = rospy.get_param("~server", "/quad1_tasks")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)

wp = [[0., 0., 0.5, 0.0], [0., 2., 0.5, 1.57], [2., 0., 0.5, 1.57],
      [0., -2., 0.5, 3.14], [-2., 0., 0.5, -1.57], [0., 0., 0.5, 0.0]]

try:
    tc.SetMotor(on=True)
    tc.TakeOff()
    for p in wp:
        tc.GoTo(goal_x=p[0], goal_y=p[1], goal_z=p[2])
        tc.SetHeading(goal_heading=p[3])

    tc.Land()
    tc.SetMotor(on=False)
except:
    traceback.print_exc()
    rospy.spin()
Esempio n. 29
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

smart = True
vel = 0.5
ang_vel = 3

tc.WaitForAuto()
try:
    tc.GoToPose(goal_x=2,
                goal_y=-0.5,
                goal_theta=0.5 * 3.1415,
                max_velocity=vel,
                max_angular_velocity=ang_vel,
                smart=smart,
                sigma2=5)
    tc.Wait(duration=1.0)
    tc.GoToPose(goal_x=0.8,
                goal_y=2,
                goal_theta=3.1415,
Esempio n. 30
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

ang = 30

ang = ang * pi / 180

while True:
    try:
        tc.clearConditions()
        w4face = tc.WaitForFace(foreground=False)
        tc.addCondition(ConditionIsCompleted("Face Detector", tc, w4face))
        try:
            tc.Wander(foreground=True, front_sector=False, angular_range=ang)
        except TaskConditionException, e:
            tc.StareAtFace()

            # tc.SetPen(on=False)
            # tc.GoTo(goal_x=1.0,goal_y=1.0)
            # tc.Clear()
Esempio n. 31
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

tc.WaitForAuto()
try:
    tc.GoTo(goal_x=-1.0,goal_y=0.0)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=0.0,goal_y=1.0)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=1.0,goal_y=0.0)
    tc.Wait(duration=1.0)
    tc.GoTo(goal_x=0.0,goal_y=-1.0)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))

if not rospy.core.is_shutdown():
    tc.SetManual()


rospy.loginfo("Mission completed")
Esempio n. 32
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

scale=2.0
vel=0.5

tc.WaitForAuto()
try:
    
    while(True):
        # Prepare a condition so that the following gets executed only until the 
        # Region of Interest is found
        # w4face = tc.WaitForFace(foreground = False)
        # tc.addCondition(ConditionIsCompleted("Face detector",tc,w4face))
        # tc.Wander()
        # Start the wait for roi task in the background
        w4face = tc.WaitForFace(foreground = False)
        tc.addCondition(ConditionIsCompleted("Face detector",tc,w4face))
        
        try:
            tc.Wander()
Esempio n. 33
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

scale = 1.5
vel = 1
angle = 1.5

tc.WaitForAuto()
try:
    tc.GoToPose(goal_x=3,
                goal_y=3,
                max_velocity=vel,
                goal_angle=1,
                dumbSmart=False,
                k_v=3,
                k_alpha=8,
                k_gamma=-1.5,
                Holonomic=True)
    tc.Wait(duration=1.0)
Esempio n. 34
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph.picklez")

tc.WaitForAuto()
try:
    while True:
        tc.Wait(duration=1.0)
        # Hand-made hamiltonian path
        node_seq = [2, 1, 4, 3, 5, 6, 0, 9, 8, 10, 7, 11]
        for node in node_seq:
            tc.GoTo(goal_x=g.vs[node]["x"],
                    goal_y=g.vs[node]["y"],
                    max_velocity=0.5,
                    k_v=1.0,
                    max_angular_velocity=1.0)
        tc.Wait(duration=1.0)
Esempio n. 35
0
#!/usr/bin/python
# ROS specific imports
import roslib

roslib.load_manifest('floor_nav')
import rospy
import tf
from math import *
from task_manager_lib.TaskClient import *
from std_msgs.msg import Empty, Bool
from geometry_msgs.msg import PoseStamped

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

pose_pub = rospy.Publisher("/planner/explore", Empty, queue_size=1)
goto_pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)
en_pub = rospy.Publisher("/enable_avoidance", Bool, queue_size=1)


def finished_callback(msg):
    global finished
    finished = True


finished_sub = rospy.Subscriber("/explore_finished", Bool, finished_callback)