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()
except TaskConditionException as e:
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)
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])
tc.Wait(duration=2.)
tc.SetPen(on=False)
