Esempio n. 1
0
 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'
Esempio n. 2
0
 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)
Esempio n. 3
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'
Esempio n. 4
0
    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'
Esempio n. 5
0
 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)
Esempio n. 6
0
 def execute(self, userdata):
     turn(0)
     dive(0)
     forward(0)
     rospy.loginfo("motors should be reset now...")
     return super(Idle, self).execute(userdata)