roll_in_deg = float(rospy.get_param('~roll_deg', 0.))
pitch_in_deg = float(rospy.get_param('~pitch_deg', 0.))
edges = int(float(rospy.get_param('~edges', 1)))
if edges < 1:
edges = 1
rospy.loginfo(
'AP Shape Controller - swim %d-side shape @ %.1f w RP=%.1f, %.1f for %.1f sec'
% (edges, speed, roll_in_deg, pitch_in_deg, dt_in_sec))
try:
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
init_yaw_in_deg = rpy_from_imu_to_global[2] * 180 / pi
target_depth = ac.current_depth
vx = speed
vz = 0
dt = rospy.Duration(dt_in_sec / edges)
delta_yaw_in_deg = 360.0 / edges
for i in xrange(edges):
rospy.loginfo('step %d/%d: dyaw=%.1f' %
(i + 1, edges, delta_yaw_in_deg * i))
target_angles = [
roll_in_deg, pitch_in_deg,
init_yaw_in_deg + delta_yaw_in_deg * i
]
ac.do_straight_line(dt, target_angles, target_depth, vx, vz)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print e
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'
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
vx = 0.8
vz = 0
#ac.do_relative_angle_change([0, 0, 180], target_depth, vx, vz)
ta = target_angles
ta[2] = ta[2] + 180
ac.do_straight_line(dt_in_sec, ta, target_depth, vx, vz)
"""
print 'Going straight'
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
target_angles = get_target_angles(rpy_from_imu_to_global, roll_in_deg)
dt_in_sec = rospy.Duration(10)
vx = 0.8
delta_yaw_in_deg = float(rospy.get_param('~delta_yaw_deg', 0.))
try:
rospy.loginfo(
'Aqua Coverage Controller - swim @ %.1f w RPdeltaY=%.1f, %.1f, %.1f for %.1f sec'
% (speed, roll_in_deg, pitch_in_deg, delta_yaw_in_deg, dt_in_sec))
rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
curr_yaw_in_deg = rpy_from_imu_to_global[2] * 180 / pi
target_depth = ac.current_depth
target_angles = [
roll_in_deg, pitch_in_deg, curr_yaw_in_deg + delta_yaw_in_deg
]
vx = speed
vz = 0
ac.do_straight_line(rospy.Duration(dt_in_sec), target_angles,
target_depth, vx, vz)
nsteps = 5
delta_yaw = 180. / nsteps
vx = speed / 2
vz = 0
for i in range(nsteps):
rospy.loginfo(
'Aqua Coverage Controller - Turn %.1f @ speed (step %d/%d)' %
(delta_yaw, vx, i, nsteps))
ac.do_relative_angle_change([0, 0, delta_yaw], target_depth, vx,
vz)
rospy.loginfo(
'Aqua Coverage Controller - Swim Straight @ speed %.1f for %.1f sec (step %d/%d)'
% (vx, 1, i, nsteps))