def execute(self, userdata):
self.publisher = rospy.Publisher('/Module_Enable', ModuleEnableMsg)
self.subscriber = rospy.Subscriber('/Task_Completion', String, self.taskCompleted)
self.buoySubscriber = rospy.Subscriber('img_rec/buoys/red', ImgRecObject, self.buoyLoc)
self.depthSubscriber = rospy.Subscriber('Sub_Depth', Float32, self.depth)
msg = ModuleEnableMsg()
msg.Module = 'NewBuoyTask'
msg.State = True
self.publisher.publish(msg)
#keep trying until preempted, success, or timeout
while self.timeout > 0:
if self.buoyHit:
self.beDone()
return 'succeeded'
if self.preempt_requested():
self.beDone()
self.service_preempt()
return 'preempted'
if self.objectLost:
self.scanForBuoy()
else:
self.descendToBuoy()
self.alignWithBouy()
self.advance()
self.extendTimeout()
# we decide the object is lost until we receive another message
self.objectLost = True
rospy.sleep(1)
self.timeout -= 1
self.objectLost = True
#we timed out
self.beDone()
return 'timeout'
def execute(self, userdata):
self.pub = rospy.Publisher("/Module_Enable", ModuleEnableMsg)
self.sub = rospy.Subscriber("/Task_Completion", String, self.task_complete)
msg = ModuleEnableMsg()
msg.Module = "PathTask"
msg.State = True
self.pub.publish(msg)
for i in range(100):
if self.is_complete:
self.disable_task()
return "succeeded"
if self.preempt_requested():
self.disable_task()
self.service_preempt()
return "preempted"
rospy.sleep(1)
self.disable_task()
return "succeeded"
def execute(self, userdata):
self.pub = rospy.Publisher('/Module_Enable', ModuleEnableMsg)
self.sub = rospy.Subscriber('/Task_Completion', String, self.task_complete)
self.objSub = rospy.Subscriber('img_rec/paths', ImgRecObject, self.objCallback)
msg = ModuleEnableMsg()
msg.Module = 'NewPathTask'
msg.State = True
self.pub.publish(msg)
dive(4)
while self.timeout>0:
if self.is_complete:
self.disable_task()
return 'succeeded'
if self.preempt_requested():
self.disable_task()
self.service_preempt()
return 'preempted'
forward(0.35)
rospy.sleep(1)
self.timeout-=1
self.disable_task()
return 'timeout'
def beDone(self):
msg = ModuleEnableMsg()
msg.Module = 'NewBuoyTask'
msg.State = False
self.publisher.publish(msg)
def disable_task(self):
msg = ModuleEnableMsg()
msg.Module = "PathTask"
msg.State = False
self.pub.publish(msg)
