Example #1
0
class BumpIntoBuoy(smach.State):
    def __init__(self):
        print("bumping")
        smach.State.__init__(self, outcomes=['Success', 'Failed'])
        self.smach_pub = rospy.Publisher('task_desiredAction',
                                         task_desiredAction,
                                         queue_size=10)
        self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
        self.gnc_sub = Subscribe_to('controlCommand')
        self.task = task_desiredAction()
        self.counter = 0
        time.sleep(0.1)

    def execute(self, userdata):
        self.task.bumpIntoBuoy = True
        self.smach_pub.publish(self.task)
        while True:
            self.sensors_data = self.sensors_sub.get_data()
            self.gnc_data = self.gnc_sub.get_data()
            if (self.sensors_data.stabilized and self.gnc_data.final_command):
                return 'Success'
            #Change this according to the GNC's bumpIntoBuoy function
            self.counter = self.counter + 1
            time.sleep(0.01)
            if (self.counter > 2000):
                return 'Failed'
class CenterWithBuoy(smach.State):
    def __init__(self):
        print("centering")
        smach.State.__init__(self, outcomes=['Success', 'Failed'])
        self.smach_pub = rospy.Publisher('task_desiredAction',
                                         task_desiredAction,
                                         queue_size=10)
        self.cv_sub = Subscribe_to('target')
        self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
        self.task = task_desiredAction()
        self.centered = False
        self.counter = 0
        time.sleep(0.1)

    def execute(self, userdata):
        self.cv_data = self.cv_sub.get_data()
        while self.cv_data.buoy1:
            self.cv_data = self.cv_sub.get_data()
            self.sensors_data = self.sensors_sub.get_data()
            self.center()
            # X less than 5 degrees and Y less than 6 inches (Test These Numbers)
            if ((abs(self.cv_data.buoy1x) < 5)
                    and (abs(self.cv_data.buoy1y) < 0.1524)):
                self.task.yaw_set = 0
                self.task.depth_set = 0
                self.smach_pub.publish(self.task)
                if (self.sensors_data.stabilized):
                    self.centered = True
            else:
                self.centered = False

            if self.centered:
                #Center 1 meter from the Buoy
                self.task.distance_set = self.cv_data.buoy1_distance - 1
                self.smach_pub.publish(self.task)
                if (abs(self.cv_data.buoy1_distance) < 0.1524):
                    self.task.distance_set = 0
                    self.smach_pub.publish(self.task)
                    if (self.sensors_data.stabilized):
                        return 'Success'
            self.counter = self.counter + 1
            time.sleep(0.01)
            if (self.counter > 6000):
                return 'Failed'
        return 'Failed'

    def center(self):
        self.task.yaw_set = self.cv_data.buoy1x
        self.task.depth_set = self.cv_data.buoy1y
        #In case we become uncentered
        if not (self.centered):
            self.task.distance_set = 0
        self.smach_pub.publish(self.task)
Example #3
0
class Turn_While_Detecting(smach.State):
    def __init__(self):
        print("starting")
        smach.State.__init__(self, outcomes=['Success', 'Failed'])

        self.cameras_sub = Subscribe_to('cameras2smach')
        self.controls_sub = Subscribe_to('controls2smach')
        self.flag = 0
        self.counter = 0
        self.controls_angle = smach2controls()
        sleep(2)

    def execute(self, userdata):
        self.cameras_data = self.cameras_sub.get_data()
        print(self.cameras_data)

        while (self.cameras_data.something == 0
               ):  #While there is no line detected
            sleep(0.01)
            self.cameras_data = self.cameras_sub.get_data()
            self.controls_data = self.controls_sub.get_data()
            print(self.cameras_data, self.controls_data)

            if (self.flag == 0):
                self.controls_angle.yaw_setpoint = -45
                smach_2_controls_pub.publish(self.controls_angle)
                self.flag = 1

            elif ((self.flag == 1) and (self.controls_data.stabilized == True)
                  and (self.counter < 200)):
                self.counter = self.counter + 1
            elif ((self.flag == 1) and (self.counter == 200)):
                self.controls_angle.yaw_setpoint = 90
                smach_2_controls_pub.publish(self.controls_angle)
                self.flag = 2

            elif ((self.flag == 2) and (self.controls_data.stabilized == True)
                  and (self.counter < 400)):
                self.counter = self.counter + 1
            elif ((self.flag == 2) and (self.counter == 400)):
                return 'Failed'

        return 'Success'
Example #4
0
class Lane_Detection(smach.State):
	def __init__(self):
		print("starting")
		smach.State.__init__(self, outcomes=['Success', 'Failed'])
		self.cameras_sub = Subscribe_to('cameras2smach')
		self.counter = 0
		sleep(2)

	def execute(self, userdata):
		self.cameras_data = self.cameras_sub.get_data()
		print (self.cameras_data)

		while (self.cameras_data.something == 0):
			sleep(0.01)

			self.cameras_data = self.cameras_sub.get_data()
			print (self.cameras_data)
			if (self.counter > 500):
				return 'Failed'
			self.counter = self.counter + 1

		return 'Success'
class SearchForBuoy(smach.State):
	def __init__(self):
		print("searching")
		smach.State.__init__(self, outcomes=['Success', 'Failed'])
		self.smach_pub = rospy.Publisher('task_desiredAction', task_desiredAction, queue_size=10)
		self.cv_sub = Subscribe_to('target')
		self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
		self.counter = 0
		self.task = task_desiredAction()
		time.sleep(2)

	def execute(self, userdata):
		#Assume Robot starts with all setpoints at 0
		print("Dive")
		self.task.depth_set = 1
		self.smach_pub.publish(self.task)
		time.sleep(0.5)
		self.sensors_data = self.sensors_sub.get_data()
		while not self.sensors_data.stabilized:
			time.sleep(0.01)
			self.counter = self.counter + 1
			if (self.counter > 1500):
				return 'Failed'
			self.sensors_data = self.sensors_sub.get_data()

		#Turn 45
		print("Turn 45")
		self.counter = 0
		self.task.depth_set = 0
		self.task.yaw_set = 45
		self.smach_pub.publish(self.task)
		time.sleep(0.5)
		self.sensors_data = self.sensors_sub.get_data()
		while not self.sensors_data.stabilized:
			time.sleep(0.01)
			self.counter = self.counter + 1
			if (self.counter > 1500):
				return 'Failed'
			self.sensors_data = self.sensors_sub.get_data()

		#Forward until see Buoy
		print("Forwards")
		self.counter = 0
		self.task.yaw_set = 0
		self.task.distance_set = 5
		self.smach_pub.publish(self.task)
		self.cv_data = self.cv_sub.get_data()
		while not self.cv_data.buoy1:
			time.sleep(0.01)
			self.counter = self.counter + 1
			if (self.counter > 2000):
				return 'Failed'
			self.cv_data = self.cv_sub.get_data()

		print("Buoy Found")
		self.task.distance_set = 0
		self.smach_pub.publish(self.task)
		return 'Success'
class Waypoint_Navigation(smach.State):
    def __init__(self):
        print("starting")
        smach.State.__init__(self,
                             outcomes=['Finished', 'Failed'],
                             input_keys=['goal'])

        self.sensors_sub = Subscribe_to('sensors2smach')
        self.controls_pub = rospy.Publisher('smach2controls',
                                            smach2controls,
                                            queue_size=1)
        self.setpoints = smach2controls()
        self.counter = 0
        time.sleep(2)

    def execute(self, userdata):
        sensors_data_sent = self.sensors_sub.was_data_sent()
        print(sensors_data_sent)
        while (sensors_data_sent == False
               ):  #If no data received in 6 seconds, Failed
            time.sleep(0.01)
            sensors_data_sent = self.sensors_sub.was_data_sent()
            if (self.counter > 600):
                return 'Failed'
            self.counter = self.counter + 1
        sensors_data = self.sensors_sub.get_data()

        goal_lat = 34.066010  #34 degs, 03.9606 mins (N) || 34 degs, 03 mins, 57.6 s (N)
        goal_lon = -118.167300  #118 degs, 10.0380 mins (W) || 118 degs, 10 mins, 02.3 s (W)

        current_lat_deg_part = int(str(sensors_data.current_latitude)[:2])
        current_lat_min_part = float(str(sensors_data.current_latitude)[2:])
        current_lat = (current_lat_deg_part + (current_lat_min_part / 60)
                       )  #In pure degrees

        current_lon_deg_part = int(str(sensors_data.current_longitude)[:3])
        current_lon_min_part = float(str(sensors_data.current_longitude)[3:])
        current_lon = (current_lon_deg_part + (current_lon_min_part / 60)
                       )  #In pure degrees

        current_lon = (current_lon * (-1)
                       )  #We will be dealing with the West values only
        Y_error = (goal_lat - current_lat
                   )  #Y axis is Earth's N & S (unit of Lat degrees)
        X_error = (goal_lon - current_lon
                   )  #X axis is Earth's E & W (unit of Lon degrees)
        #For Reference: one side of a field to another is about .001 degrees
        #One degree of Latitude remains mostly constant at 364,000 ft or ~110947.2 meters
        #One degree of Longitude depends on Latitude and average constant of 11319.488 meters
        #USGS says one deg of Lon at 38 degrees Lat is 288,200 ft or ~87843.36 meters
        Y_error = (Y_error * 110947.2
                   )  #Convert units of Lat & Lon Degrees to real meters
        X_error = (X_error * (111319.488 *
                              (math.cos(math.radians(current_lat)))))
        print Y_error
        print X_error

        ang_from_Earths_X = math.degrees(math.atan(Y_error / X_error))
        if (X_error < 0):  #atan returns only between -90 to 90
            ang_from_Earths_X = ang_from_Earths_X + 180
        elif (Y_error < 0):
            ang_from_Earths_X = ang_from_Earths_X + 360
            #this makes sure that the correct angles are returned
        ang_from_Earths_M = ang_from_Earths_X - 102  #Mag field (M) is 12 degrees left of Y
        if (ang_from_Earths_M < 0):
            ang_from_Earths_M = ang_from_Earths_M + 360
        CW_ang_M = ((ang_from_Earths_M * (-1)) + 360
                    )  #Change CCW to CW angle from Earths M
        ang_Error = CW_ang_M - sensors_data.current_mag_yaw  #Calculate ang Error to turn
        self.setpoints.yaw_setpoint = sensors_data.current_yaw + ang_Error
        #Setpoint = Current Angle + Error
        if (self.setpoints.yaw_setpoint > 360):
            self.setpoints.yaw_setpoint = self.setpoints.yaw_setpoint - 360
        elif (self.setpoints.yaw_setpoint < 0):
            self.setpoints.yaw_setpoint = self.setpoints.yaw_setpoint + 360

        #self.setpoints.distance_setpoint = 0 #Dont move forward, only turn to angle for now
        self.setpoints.distance_setpoint = math.sqrt((Y_error**2) +
                                                     (X_error**2))

        self.controls_pub.publish(self.setpoints)
        time.sleep(1)
        return 'Finished'
class Guidance_Navigation_Control():
	def __init__(self):
		print("starting")
		self.smach_sub = Subscribe_to('task_desiredAction')
		self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
		self.setpoints = controlCommand()


	def data_received(self):
		if (self.sensors_sub.was_data_sent()):
			if (self.smach_sub.was_data_sent()):
				return True
			else:
				self.setpoints.distance_set = 0
				gnc_pub.publish(self.setpoints)
				return False
		return False


	def main_loop(self):
		print("looping")
		if self.smach_sub.was_data_sent():
			print("data_updated")
			self.smach_data = self.smach_sub.get_data()
			self.sensors_data = self.sensors_sub.get_data()
			if (self.smach_data.surface):
				self.surface()
			elif (self.smach_data.bumpIntoBuoy):
				self.bumpIntoBuoy()
			self.update_setpoints()
			gnc_pub.publish(self.setpoints)


	def update_setpoints(self):
		print("updating")
		self.setpoints.yaw_set = self.smach_data.yaw_set + self.sensors_data.yaw_current
		self.setpoints.pitch_set = self.smach_data.pitch_set + self.sensors_data.pitch_current
		self.setpoints.roll_set = self.smach_data.roll_set + self.sensors_data.roll_current
		self.setpoints.depth_set = self.smach_data.depth_set + self.sensors_data.depth_current
		self.setpoints.distance_set = self.smach_data.distance_set


	def surface(self):
		print("surfacing")
		self.setpoints.yaw_set = self.sensors_data.yaw_current
		self.setpoints.pitch_set, self.setpoints.roll_set = 0, 0
		self.setpoints.distance_set, self.setpoints.depth_set = 0, 0
		self.setpoints.final_command = True
		gnc_pub.publish(self.setpoints)
		sleep(10)
		exit()


	def bumpIntoBuoy(self):
		print("bumping")
		#Forwards
		self.setpoints.distance_set = 5
		gnc_pub.publish(self.setpoints)
		sleep(4)
		#Stop
		self.setpoints.distance_set = 0
		gnc_pub.publish(self.setpoints)
		sleep(1)
		#Backwards
		self.setpoints.distance_set = -5
                gnc_pub.publish(self.setpoints)
                sleep(4)
		#Stop
		self.setpoints.distance_set = 0
		gnc_pub.publish(self.setpoints)
		sleep(1)
		self.setpoints.final_command = True
		gnc_pub.publish(self.setpoints)