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")
while True: tc.Wait(duration=1.) tc.GoTo(goal_x=1.0,goal_y=1.0) # 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.GoTo(goal_x=p[0],goal_y=p[1]) # Clear the conditions if we reach this point tc.clearConditions() except TaskConditionException, e: rospy.loginfo("Path following interrupted on condition: %s" % \ " or ".join([str(c) for c in e.conditions])) # This means the conditions were triggered. We need to react to it # Conditions are cleared on trigger tc.ReachAngle(target=pi/2) # Follow up with normal execution tc.Wait(duration=2.) tc.GoTo(goal_x=5.0,goal_y=5.0) tc.ReachAngle(target=pi/2)