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