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)
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)
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
#!/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)
#!/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
#!/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")
#!/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))
#!/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()
#!/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)
#!/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))
#!/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))
#!/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
#!/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)
#!/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))
#!/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")
#!/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])
#!/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")
#!/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()
#!/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,
#!/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()
#!/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")
#!/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()
#!/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)
#!/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)
#!/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)