#!/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")
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))
if not rospy.core.is_shutdown():
tc.SetManual()
#!/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('task_manager_turtlesim_sync')
import rospy
from math import *
from task_manager_lib.TaskClient import *
rospy.init_node('task_client3')
server_node = rospy.get_param("~server", "/turtlesim_tasks3")
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.GoTo(goal_x=1.0, goal_y=9.0)
tc.SetStatusSync(status=1)
tc.WaitForStatusSync(partner="partner1", status=0)
tc.WaitForStatusSync(partner="partner2", status=0)
tc.GoTo(goal_x=1.0, goal_y=1.0)
tc.SetStatusSync(status=0)
rospy.loginfo("Mission3 completed")
while True:
# spotMarkers = uav.SpotMarkers(foreground = False);
w4landingBackOrder = uav.WaitForLandingBackOrder(foreground=False)
w4lowBatteryLevel = uav.WaitForLowBatteryLevel(foreground=False)
uav.addCondition(
ConditionIsCompleted("Landing Back Order", uav,
w4landingBackOrder))
uav.addCondition(
ConditionIsCompleted("Low Battery Level", uav, w4lowBatteryLevel))
try:
while True:
p = wp[i]
uav.GoTo(goal_x=p[0],
goal_y=p[1],
goal_z=2.5,
max_velocity=0.3)
uav.Wait(duration=1.0)
if (i < 7):
i += 1
else:
uav.RequestLanding()
uav.Wait(duration=2.0)
uav.LandingBack()
except TaskConditionException, e:
if uav.isCompleted(w4landingBackOrder):
rospy.loginfo(
"Mission suspended - UGV ordered a Landing Back !")
uav.RequestLanding()
uav.Wait(duration=2.0)
uav.LandingBack()
tc.SetMotor(on=True)
tc.TakeOff()
# Wait for other quad to have taken off
tc.SetStatusSync(status=mc.QUAD_FLYING)
tc.WaitForStatusSync(partner="partner2", status=mc.QUAD_FLYING)
w4partner = tc.WaitForStatusSync(foreground=False,
partner="partner2",
status=mc.QUAD_CALLING)
tc.addCondition(ConditionIsCompleted("Partner2", tc, w4partner))
w4battery = tc.WaitForLowBattery(foreground=False, threshold=70.0)
tc.addCondition(ConditionIsCompleted("Battery", tc, w4battery))
try:
for i, p in enumerate(wp):
tc.GoTo(goal_x=p[0], goal_y=p[1], goal_z=p[2])
if p[3]:
tc.RadioScan(num_sectors=3, acquisition_time=2.0)
tc.SetStatusSync(status=mc.QUAD_DOWN)
tc.clearConditions()
except TaskConditionException, e:
cnames = [str(c) for c in e.conditions]
if "Partner2" in cnames:
# We've been called...
rospy.loginfo(
"Partner2 requested intervention, interrupting mission")
tc.GoTo(goal_x=p2.linear.x, goal_y=p2.linear.y, goal_z=1.0)
tc.RadioScan(num_sectors=3, acquisition_time=5.0)
tc.SetStatusSync(status=mc.QUAD_DONE_HELPING)
tc.GoTo(goal_x=wp[-1][0], goal_y=wp[-1][1], goal_z=wp[-1][2])
uav = TaskClient(server_node_uav, default_period_uav)
rospy.loginfo("Mission connected to UAV server: " + server_node_uav)
# ugv task client
server_node_ugv = rospy.get_param("~server", "/bubblerob_server")
default_period_ugv = rospy.get_param("~period", 0.05)
ugv = TaskClient(server_node_ugv, default_period_ugv)
rospy.loginfo("Mission connected to UGV server: " + server_node_ugv)
# press red button to launch the mission
ugv.WaitForAuto()
uav.SetMotor(on=True)
ugv.SetSuction(active=False)
uav.GoTo(goal_x=0, goal_y=0, goal_z=1)
ugv.GoTo(goal_x=-1, goal_y=-1)
uav.GoTo(goal_x=-1, goal_y=-1, goal_z=1)
ugv.GoTo(goal_x=1, goal_y=-1)
uav.GoTo(goal_x=1, goal_y=-1, goal_z=1)
# Landing
ugv.SetSuction(active=True)
time.sleep(5)
uav.GoTo(goal_x=1, goal_y=-1, goal_z=0.4)
time.sleep(5)
uav.SetMotor(on=False)
rospy.loginfo("Mission completed")
uav.TakeOff()
while True:
w4landingBackOrder = uav.WaitForLandingBackOrder(foreground=False)
w4lowBatteryLevel = uav.WaitForLowBatteryLevel(foreground=False)
uav.addCondition(
ConditionIsCompleted("Landing Back Order", uav,
w4landingBackOrder))
uav.addCondition(
ConditionIsCompleted("Low Battery Level", uav, w4lowBatteryLevel))
try:
while True:
p = wp[i]
uav.GoTo(goal_x=p[0], goal_y=p[1], goal_z=1)
uav.Wait(duration=1.0)
if (i < 4):
i += 1
else:
i = 0
except TaskConditionException, e:
if uav.isCompleted(w4landingBackOrder):
rospy.loginfo(
"Mission suspended - UGV ordered a Landing Back !")
uav.RequestLanding()
uav.Wait(duration=2.0)
uav.LandingBack()
uav.WaitForTakeOffOrder()
uav.RequestTakeOff()
uav.Wait(duration=2.0)
tc.SetMotor(on=True)
tc.TakeOff()
# Wait for other quad to have taken off
tc.SetStatusSync(status=mc.QUAD_FLYING)
tc.WaitForStatusSync(partner="partner1", status=mc.QUAD_FLYING)
w4partner = tc.WaitForStatusSync(foreground=False,
partner="partner1",
status=mc.QUAD_CALLING)
tc.addCondition(ConditionIsCompleted("Partner1", tc, w4partner))
w4battery = tc.WaitForLowBattery(foreground=False, threshold=70.0)
tc.addCondition(ConditionIsCompleted("Battery", tc, w4battery))
try:
for i, p in enumerate(wp):
tc.GoTo(goal_x=p[0], goal_y=p[1], goal_z=p[2])
if p[3]:
tc.RadioScan(num_sectors=3, acquisition_time=2.0)
# Trigger an help signal after the 3rd waypoint and wait for the
# other quad to have finished scanning
if i == 3:
tc.SetStatusSync(status=mc.QUAD_CALLING)
tc.WaitForStatusSync(partner="partner1",
status=mc.QUAD_DONE_HELPING)
tc.SetStatusSync(status=0)
tc.clearConditions()
except TaskConditionException, e:
cnames = [str(c) for c in e.conditions]
try:
tc.WaitForAuto()
while True:
w4takeOffReq = tc.WaitForTakeOffRequest(foreground=False)
w4landingReq = tc.WaitForLandingRequest(foreground=False)
tc.addCondition(
ConditionIsCompleted("takeOff Request", tc, w4takeOffReq))
tc.addCondition(
ConditionIsCompleted("Landing Request", tc, w4landingReq))
try:
while True:
p = wp[i]
tc.GoTo(goal_x=p[0], goal_y=p[1], max_velocity=maxVel)
tc.Wait(duration=1.0)
if (i < 4):
i += 1
else:
i = 0
except TaskConditionException, e:
if tc.isCompleted(w4takeOffReq):
rospy.loginfo("Mission suspended - Take-Off Requested !")
tc.WaitingForTakeOff()
if tc.isCompleted(w4landingReq):
rospy.loginfo("Mission suspended - Landing Requested !")
tc.WaitingForLanding()
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
#!/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", "/bubblerob_server")
default_period = rospy.get_param("~period", 0.2)
tc = TaskClient(server_node, default_period)
wp = [[0., 0], [2., 0.], [0., -2.], [-2., 0.], [0., 2], [0., 0.]]
tc.SetSuction(active=False)
for p in wp:
tc.Wait(duration=0.2)
tc.GoTo(goal_x=p[0], goal_y=p[1])
tc.SetHeading(target=pi / 2)
tc.SetSuction(active=True)
rospy.loginfo("Mission completed")
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()
try:
for p in wp:
tc.Wait(duration=0.2)
tc.ReachAngle(target=p[2])
tc.SetPen(on=True,r=p[3],g=p[4],b=p[5])
tc.GoTo(goal_x=p[0],goal_y=p[1],task_timeout=2.0)
except TaskException, e:
if e.status == TaskStatus.TASK_TIMEOUT:
rospy.loginfo("Path following interrupted by timeout, as expected: %s" % str(e))
tc.ReachAngle(target=pi/2)
else:
raise
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.0
vel = 0.75
ang = pi / 2
#tc.WaitForAuto()
try:
tc.GoTo(goal_x=scale, goal_y=scale, theta=ang, max_velocity=vel)
#tc.Wait(duration=5.0)
#tc.GoTo(goal_x=0,goal_y=scale,theta=ang,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))
if not rospy.core.is_shutdown():
tc.SetManual()
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])
tc.SetPen(on=True, r=p[3], g=p[4], b=p[5])
tc.GoTo(goal_x=p[0], goal_y=p[1])
# Clear the conditions if we reach this point
tc.clearConditions()
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))
if not rospy.core.is_shutdown():
tc.SetManual()
rospy.loginfo("Mission completed")
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)
except:
traceback.print_exc()
rospy.spin()
rospy.loginfo("Mission completed")
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")
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)
node_seq.reverse()
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)
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
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)
scalex = 4
scaley = 0
vel = 1
tc.WaitForAuto()
try:
tc.GoTo(goal_x=scalex, goal_y=scaley, max_velocity=vel, Holonomic=True)
tc.Wait(duration=1.0)
# tc.GoTo(goal_x=1,goal_y=1,max_velocity=vel,Holonomic=True)
# tc.Wait(duration=1.0)
# tc.GoTo(goal_x=scalex,goal_y=-scaley,max_velocity=vel,Holonomic=True)
# tc.Wait(duration=1.0)
# tc.GoTo(goal_x=-scalex,goal_y=-scaley,max_velocity=vel,Holonomic=True)
# tc.Wait(duration=1.0)
# tc.GoTo(goal_x=scalex,goal_y=scaley,max_velocity=vel,Holonomic=True)
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
if not rospy.core.is_shutdown():
tc.SetManual()
