#
# ac.do_sideways_somersault(dt_in_sec, vx, vz, target_depth, -24.0)
#
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
# print e
try:
print 'SommerCork'
ac.set_mode(depth_params)
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
target_angles = [0, 0, rpy_from_imu_to_global[2] * 180 / pi]
vx = 0.8
vz = 0.0
dt_in_sec = rospy.Duration(15.0)
ac.do_rolling_sideways_somersault(dt_in_sec, vx, vz, target_depth,
24.0)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print e
# try:
# print 'SommerSide'
# ac.set_mode(depth_params)
# rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
# target_angles = [0, 0, rpy_from_imu_to_global[2]*180/pi]
# vx = 0.9
# vz = 0.0
# dt_in_sec = rospy.Duration(20.0)
#
# ac.do_sideways_somersault(dt_in_sec, vx, vz, target_depth, 18.0)
class ShipProgram(object):
def __init__(self, speed, forward_swim_time, sea_floor_depth, deck_depth,
hold_depth, tag_dead_zone, do_swimmer_turn):
rospy.loginfo('In ShipProgram init')
self.depth_params = {}
self.depth_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
self.angle_params = {}
self.angle_params[
'mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
self.ac = AutopilotClient(self.depth_params)
self.client = dynamic_reconfigure.client.Client("/localAP", timeout=5)
self.knife_edge_params = {
'ROLL_P_GAIN': 1.0,
'PITCH_P_GAIN': 2.0,
'YAW_P_GAIN': -3.5,
'ROLL_D_GAIN': 0.0,
'PITCH_D_GAIN': 0.0,
'YAW_D_GAIN': -1.5,
'KDEPTH': -1.3,
'DEPTH_D_GAIN': -1.5,
'DEPTH_D_FILTER_PERIOD': 0.6
}
self.behaviour_dict = {
0: ['flip_turn', 'forward'],
1: ['right_turn', 'forward'],
2: ['knife_edge', 'forward'],
3: ['flatten', 'forward'],
4: ['left_turn', 'forward'],
5: ['park']
# [ 'up_to_deck', 'forward' ],
# [ 'forward_timed', 'down_to_sea_floor', 'forward' ]
# [ 'dance', 'forward' ]
# [ 'forward_timed', 'down_to_hold', 'forward_timed', 'up_to_deck', 'forward]
}
self.client.update_configuration(self.knife_edge_params)
self.ac.set_mode(self.depth_params)
self.target_depth = self.ac.current_depth
self.rpy_from_imu_to_global = self.ac.get_rpy_of_imu_in_global()
self.current_roll_deg = self.rpy_from_imu_to_global[0] * 180.0 / pi
self.current_pitch_deg = self.rpy_from_imu_to_global[1] * 180.0 / pi
self.current_yaw_deg = self.rpy_from_imu_to_global[2] * 180.0 / pi
# Choose a trajectory file based on parameter and send it to the ap_client function
self.active_command = None
self.command_list = []
self.command_endtime = None
# TODO make params
self.sea_floor_depth = sea_floor_depth
self.deck_depth = deck_depth
self.hold_depth = hold_depth
self.forward_swim_time = forward_swim_time
self.speed = speed
self.tag_dead_zone = tag_dead_zone
self.do_swimmer_turn = do_swimmer_turn
self.last_tag_time = None
rospy.Subscriber("/tags", Tags, self.tag_callback)
rospy.Subscriber("/aqua/state", StateMsg, self.state_callback)
rospy.loginfo('Finished constructor')
def clamp180(self, angle):
while angle > 180.0:
angle = angle - 360.0
while angle < -180.0:
angle = angle + 360.0
return angle
def tag_callback(self, tag):
rospy.loginfo('Tag callback.')
if len(tag.tags) > 0:
rospy.loginfo('Saw a tag.')
else:
return
# A small delay after seeing a tag so that double detections dont hurt us
if (self.last_tag_time == None
) or (rospy.Time.now() - self.last_tag_time) > rospy.Duration(
self.tag_dead_zone):
self.last_tag_time = rospy.Time.now()
# Handle the tag based on its ID:
if not self.behaviour_dict.has_key(tag.tags[0].id):
rospy.loginfo('Unrecognized tag: ' + str(tag.tags[0].id) +
' detected.')
else:
self.command_list.extend(self.behaviour_dict[tag.tags[0].id])
rospy.loginfo('Read tag: ' + str(tag.tags[0].id) +
' to make action list: ' +
str(self.command_list))
def state_callback(self, event):
rospy.loginfo('State callback. Current command is: ' +
str(self.active_command))
new_command = False
if self.active_command == None:
new_command = True
else:
if self.active_command == 'up_to_deck':
if self.ac.current_depth <= self.deck_depth:
new_command = True
elif self.active_command == 'forward_timed':
if self.command_endtime <= rospy.Time.now():
new_command = True
elif self.active_command == 'down_to_sea_floor':
if self.ac.current_depth >= self.sea_floor_depth:
new_command = True
elif self.active_command == 'down_to_hold':
if self.ac.current_depth >= self.hold_depth:
new_command = True
else:
rospy.loginfo('Error, command unknown: ' +
str(self.active_command))
if new_command:
if len(self.command_list) > 0:
self.active_command = self.command_list.pop(0)
if self.active_command == 'forward':
# Implementation of "flat swim"
rospy.loginfo('Switching to command forward')
# The behaviour is immediate, so can unset active command
self.active_command = None
self.current_pitch_deg = 0.0
self.ac.set_mode(self.depth_params)
if self.active_command == 'right_turn':
self.current_yaw_deg = self.current_yaw_deg - 90.0
# The behaviour is immediate, so can unset active command
self.active_command = None
self.ac.set_mode(self.depth_params)
if self.active_command == 'left_turn':
self.current_yaw_deg = self.current_yaw_deg + 90.0
# The behaviour is immediate, so can unset active command
self.active_command = None
self.ac.set_mode(self.depth_params)
if self.active_command == 'knife_edge':
self.current_roll_deg = 90.0
# The behaviour is immediate, so can unset active command
self.active_command = None
self.ac.set_mode(self.depth_params)
if self.active_command == 'flatten':
self.current_roll_deg = 0.0
# The behaviour is immediate, so can unset active command
self.active_command = None
self.ac.set_mode(self.depth_params)
if self.active_command == 'dance':
self.ac.set_mode(self.depth_params)
self.ac.do_rolling_sideways_somersault(
rospy.Duration(20), 0.8, 0.0, self.ac.current_depth,
18.0)
self.active_command = None
if self.active_command == 'flip_turn':
# This is the implementation of the awkward turn. Just 180 yaw and let the robot swim
rospy.loginfo('Switching to command flipturn')
if self.do_swimmer_turn:
self.ac.flip_turn(4, self.speed, self.current_yaw_deg)
self.current_yaw_deg = self.current_yaw_deg + 180.0
self.ac.set_mode(self.depth_params)
# The behaviour is immediate, so can unset active command
self.active_command = None
elif self.active_command == 'forward_timed':
rospy.loginfo('Switching to command forward timed')
self.current_pitch_deg = 0.0
self.command_endtime = rospy.Time.now() + rospy.Duration(
self.forward_swim_time)
self.ac.set_mode(self.depth_params)
elif self.active_command == 'up_to_deck':
rospy.loginfo('Switching to command up to deck')
self.current_pitch_deg = -90.0
self.ac.set_mode(self.angle_params)
self.target_depth = self.deck_depth
elif self.active_command == 'down_to_sea_floor':
rospy.loginfo('Switching to command down to sea floor')
self.current_pitch_deg = 90.0
self.ac.set_mode(self.angle_params)
self.target_depth = self.sea_floor_depth
elif self.active_command == 'down_to_hold':
rospy.loginfo('Switching to command down to hold')
self.current_pitch_deg = 90.0
self.ac.set_mode(self.angle_params)
self.target_depth = self.hold_depth
elif self.active_command == 'park':
self.ac.set_mode(self.angle_params)
rospy.loginfo('Switching to command park')
#self.ac.park()
rospy.loginfo(
'Exiting the autopilot run because a park action is finishing. See you next year!'
)
rospy.signal_shutdown('Tag asked robot to park.')
else:
rospy.loginfo('Unreckognized action: ' +
str(self.active_command))
try:
target_angles_deg = [
self.current_roll_deg, self.current_pitch_deg,
self.current_yaw_deg
]
self.ac.send_single_command(self.speed, target_angles_deg,
self.target_depth)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print e