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