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'
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 __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)
class Initialize_Robot(smach.State):
def __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Success', 'Failed'])
#Subscribe to all nodes that publish to smach
self.cv_sub = Subscribe_to('target')
self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
self.gnc_sub = Subscribe_to('controlCommand')
self.counter = 0
time.sleep(2)
def execute(self, userdata):
#Check if all nodes have published data
print(self.cv_sub.was_data_sent(), self.sensors_sub.was_data_sent(),
self.gnc_sub.was_data_sent())
#If any of the nodes are not publishing, stay in this loop
while not (self.cv_sub.was_data_sent()
and self.sensors_sub.was_data_sent()
and self.gnc_sub.was_data_sent()):
time.sleep(0.01)
#Continue checking if all nodes have published data
print(self.cv_sub.was_data_sent(),
self.sensors_sub.was_data_sent(),
self.gnc_sub.was_data_sent())
#If any nodes have failed to publish after ~20 seconds, return failed
if (self.counter > 2000):
return 'Failed'
self.counter = self.counter + 1
#When all nodes are publishing data, return Finished
return 'Success'
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)
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)
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 __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Success', 'Failed'])
#Subscribe to all nodes that publish to smach
self.sensors_sub = Subscribe_to('sensors2smach')
self.lidar_sub = Subscribe_to('lidar2smach')
self.cameras_sub = Subscribe_to('cameras2smach')
self.controls_sub = Subscribe_to('controls2smach')
self.counter = 0
time.sleep(2)
def __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Finished', 'Failed'])
#Subscribe to all nodes that publish to smach
self.nav_sub = Subscribe_to('nav2smach')
self.cv_sub = Subscribe_to('cv2smach')
self.controls_sub = Subscribe_to('controls2smach')
self.weapons_sub = Subscribe_to('weapons2smach')
self.counter = 0
time.sleep(2)
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'
def __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Success', 'Failed'])
#Subscribe to all nodes that publish to smach
self.cv_sub = Subscribe_to('target')
self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
self.gnc_sub = Subscribe_to('controlCommand')
self.counter = 0
time.sleep(2)
class Initialize(smach.State):
def __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Finished', 'Failed'])
#Subscribe to all nodes that publish to smach
self.nav_sub = Subscribe_to('nav2smach')
self.cv_sub = Subscribe_to('cv2smach')
self.controls_sub = Subscribe_to('controls2smach')
self.weapons_sub = Subscribe_to('weapons2smach')
self.counter = 0
time.sleep(2)
def execute(self, userdata):
#Check if all nodes have published data
nav_data_sent = self.nav_sub.was_data_sent()
cv_data_sent = self.cv_sub.was_data_sent()
controls_data_sent = self.controls_sub.was_data_sent()
weapons_data_sent = self.weapons_sub.was_data_sent()
print(nav_data_sent, cv_data_sent, controls_data_sent,
weapons_data_sent)
#If any of the nodes are not publishing, stay in this loop
while ((nav_data_sent == False) or (cv_data_sent == False)
or (controls_data_sent == False)
or (weapons_data_sent == False)):
time.sleep(0.01)
#Continue checking if all nodes have published data
nav_data_sent = self.nav_sub.was_data_sent()
cv_data_sent = self.cv_sub.was_data_sent()
controls_data_sent = self.controls_sub.was_data_sent()
weapons_data_sent = self.weapons_sub.was_data_sent()
print(nav_data_sent, cv_data_sent, controls_data_sent,
weapons_data_sent)
#If any nodes have failed to publish after ~10 seconds, return Failed
if (self.counter > 1000):
return 'Failed'
self.counter = self.counter + 1
#When all nodes are publishing data, return Finished
return 'Finished'
class Initialize_Robot(smach.State):
def __init__(self):
print("starting")
smach.State.__init__(self, outcomes=['Success', 'Failed'])
#Subscribe to all nodes that publish to smach
self.sensors_sub = Subscribe_to('sensors2smach')
self.lidar_sub = Subscribe_to('lidar2smach')
self.cameras_sub = Subscribe_to('cameras2smach')
self.controls_sub = Subscribe_to('controls2smach')
self.counter = 0
time.sleep(2)
def execute(self, userdata):
#Check if all nodes have published data
sensors_data_sent = self.sensors_sub.was_data_sent()
lidar_data_sent = self.lidar_sub.was_data_sent()
cameras_data_sent = self.cameras_sub.was_data_sent()
controls_data_sent = self.controls_sub.was_data_sent()
print(sensors_data_sent, lidar_data_sent, cameras_data_sent,
controls_data_sent)
#If any of the nodes are not publishing, stay in this loop
while ((sensors_data_sent == False) or (lidar_data_sent == False)
or (cameras_data_sent == False)
or (controls_data_sent == False)):
time.sleep(0.01)
#Continue checking if all nodes have published data
sensors_data_sent = self.sensors_sub.was_data_sent()
lidar_data_sent = self.lidar_sub.was_data_sent()
cameras_data_sent = self.cameras_sub.was_data_sent()
controls_data_sent = self.controls_sub.was_data_sent()
print(sensors_data_sent, lidar_data_sent, cameras_data_sent,
controls_data_sent)
#If any nodes have failed to publish after ~15 seconds, return failed
if (self.counter > 2000):
return 'Failed'
self.counter = self.counter + 1
#When all nodes are publishing data, return Finished
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'
def __init__(self):
print("starting")
self.smach_sub = Subscribe_to('task_desiredAction')
self.sensors_sub = Subscribe_to('sensorInfo_actuatorStatus')
self.setpoints = controlCommand()
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)