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)
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'
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)