from math import pi
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
import dynamic_reconfigure.client
if __name__ == '__main__':
rospy.init_node('choreography_node')
depth_params = {}
depth_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
angle_params = {}
angle_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(angle_params)
forward_speed = 0.8
loop_duration = 20.0
print 'Vertical Loop'
yaw_rate = 0.0
pitch_rate = 360.0 / loop_duration
roll_rate = 0.0
ac.smoothly_changing_forward_trajectory(loop_duration, forward_speed,
roll_rate, pitch_rate, yaw_rate)
print 'Sideways Looping Corkscrew'
yaw_rate = 360.0 / loop_duration
roll_rate = 20.0
ac.smoothly_changing_forward_fixed_depth_trajectory(
import tf
from math import pi
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
if __name__ == '__main__':
rospy.init_node('lateral')
dt_in_sec = rospy.Duration(float(rospy.get_param('~duration', 15)))
params = {}
params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
#params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(params)
roll_in_deg = 90
pitch_in_deg = 0
try:
print 'Going laterally'
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
target_depth = ac.current_depth
target_angles = [
roll_in_deg, pitch_in_deg, rpy_from_imu_to_global[2] * 180 / pi
]
vx = 0
vz = -0.8
def get_target_angles(rpy_from_imu_to_global, roll_in_deg):
target_angles_in_deg = [
roll_in_deg, 0, rpy_from_imu_to_global[2] * 180 / pi
]
return target_angles_in_deg
if __name__ == '__main__':
rospy.init_node('uturn')
params = {}
params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
#params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(params)
roll_in_deg = 0
try:
print 'Going straight'
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
target_depth = ac.current_depth
target_angles = get_target_angles(rpy_from_imu_to_global, roll_in_deg)
dt_in_sec = rospy.Duration(10)
vx = 0.8
vz = 0
ac.do_straight_line(dt_in_sec, target_angles, target_depth, vx, vz)
print 'Now turning 180 in yaw'
import roslib
roslib.load_manifest('aquaautopilot')
import rospy
import tf
from math import pi
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
if __name__ == '__main__':
rospy.init_node('corkscrew')
params = {}
params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
#params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(params)
try:
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
target_depth = ac.current_depth
wx_in_deg_per_sec = 40
dt_in_sec = rospy.Duration(30)
vx = 0.8
vz = 0
ac.do_corkscrew(dt_in_sec, target_depth, vx, vz, wx_in_deg_per_sec)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print e
roslib.load_manifest('aquaautopilot')
import rospy
import tf
from math import pi
from std_srvs.srv import Empty as EmptySrv
from aquacore.srv import IsCalibrated
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
if __name__ == '__main__':
rospy.init_node('coverage_ap_controller')
params = {}
params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
#params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(params)
# check if is calibrated
'''doesn't always work correctly in the water, and will always be manually
calibrated, so leave this out for now -- arnold '''
'''rospy.loginfo('Waiting for /aqua/calibrate and /aqua/is_calibrated...')
rospy.wait_for_service('/aqua/calibrate')
rospy.wait_for_service('/aqua/is_calibrated')
call_calibrate = rospy.ServiceProxy('/aqua/calibrate', EmptySrv)
call_is_calibrated = rospy.ServiceProxy('/aqua/is_calibrated', IsCalibrated)
try:
resp = call_is_calibrated()
if not resp.value:
rospy.loginfo('autocalibrating robot...')
call_calibrate()
else:
from math import pi
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
import dynamic_reconfigure.client
if __name__ == '__main__':
rospy.init_node('choreography_node')
depth_params = {}
depth_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
angle_params = {}
angle_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(angle_params)
client = dynamic_reconfigure.client.Client("/localAP", timeout=5)
roll_in_deg = 90
pitch_in_deg = 0
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,
from autopilot_client import AutopilotClient
import dynamic_reconfigure.client
square_side_duration = 5.0
square_side_speed = 0.9
if __name__ == '__main__':
rospy.init_node('choreography_node')
depth_params = {}
depth_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
angle_params = {}
angle_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(angle_params)
client = dynamic_reconfigure.client.Client("/localAP", timeout=5)
roll_in_deg = 90
pitch_in_deg = 0
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,
from autopilot_client import AutopilotClient
import dynamic_reconfigure.client
if __name__ == '__main__':
rospy.init_node('tag_trajectory_node')
trajectory_filename = rospy.get_param('~trajectory_filename')
depth_params = {}
depth_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_FIXED_DEPTH
angle_params = {}
angle_params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(angle_params)
client = dynamic_reconfigure.client.Client("/localAP", timeout=5 )
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 }
client.update_configuration(knife_edge_params)
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')
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
import roslib
roslib.load_manifest('aquaautopilot')
import rospy
import tf
from math import pi
from aquacore.msg import AutopilotModes
from autopilot_client import AutopilotClient
if __name__ == '__main__':
rospy.init_node('somersault')
params = {}
params['mode'] = AutopilotModes.AP_GLOBAL_ANGLES_LOCAL_THRUST
ac = AutopilotClient(params)
try:
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
wy_in_deg_per_sec = -20
dt_in_sec = rospy.Duration(30)
vx = 0.8
vz = 0
ac.do_somersault(dt_in_sec, vx, vz, wy_in_deg_per_sec)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print e
