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 descendToBuoy(self):
#dive to buoy depth
if(self.lastKnownBuoyLoc.center_y > 50):
dive(-.1)
elif(self.lastKnownBuoyLoc.center_y < -50):
dive(.1)
else:
dive(0)
def execute(self, userdata):
self.reset()
sub = rospy.Subscriber('img_rec/paths', ImgRecObject, self.found_path_cb)
dive(self._target_depth)
for i in range(self._dive_time):
if self.preempt_requested():
self.service_preempt()
sub.unregister()
return 'preempted'
rospy.sleep(1)
forward(.3)
for i in range(self._timeout):
if self.found_path:
sub.unregister()
#Start slowing down
forward(-.1)
rospy.Timer(rospy.Duration(self._slow_down_duration), self.slow_down_stop, True)
#done slowing down.
sub.unregister()
return 'found_path'
rospy.sleep(1)
sub.unregister()
self.reset()
return 'timed_out'
def execute(self, userdata):
self.reset()
self.buoy_sub = rospy.Subscriber('img_rec/buoys/red', ImgRecObject, self.buoy_cb)
self.depth_sub = rospy.Subscriber('Sub_Depth', Float32, self.depth_cb)
while True:
if self.preempt_requested():
self.service_preempt()
self.unsub()
return 'preempted'
delta = time.time() - self.last_time
if self.last_buoy and delta < self._timeout:
if abs(self.last_buoy.center_x) < 50:
self.good_x_count += 1
strafe(self.last_buoy.center_x * self._gain_x)
else:
self.good_x_count = 0
strafe(0)
if abs(self.last_buoy.center_y) < 50:
self.good_y_count += 1
dive(self.last_buoy.center_y * self._gain_y)
else:
self.good_y_count = 0
if self.last_depth:
dive(self.last_depth - self.last_buoy.center_y * self._gain_y)
if self.good_x_count >= 10 and self.good_y_count >= 10:
self.good_y_count += 1
if self.last_depth:
dive(self.last_depth)
self.last_buoy = None
elif delta > self._timeout:
self.unsub()
return 'lost_buoy'
rospy.sleep(0.5)
self.buoy_sub.unregister()
self.depth_sub.unregister()
return 'succeeded'
def scanForBuoy(self):
# we don't know where it is so let's try right
# we'll go into a circle til we time out or find the buoy
dive(0)
strafe(0)
turn(.1)
def execute(self, userdata):
turn(0)
dive(0)
forward(0)
rospy.loginfo("motors should be reset now...")
return super(Idle, self).execute(userdata)