def update_wps(self, wp_list):
if Settings.enable_autopilot:
if len(wp_list) < 2:
return
with self.wp_update_in_progress:
if LosSettings.enable_los:
if self.los_thread.isAlive():
self.los_restart_event.set()
self.los_stop_event.set()
self.los_thread.join()
self.los_stop_event.clear()
self.los_restart_event.clear()
self.los_controller.update_wps(wp_list)
self.los_controller.update_pos(self.cur_pos_msg)
self.los_thread = threading.Thread(
target=self.los_controller.loop, daemon=True)
self.los_thread.start()
self.los_start_event.set()
return
thread = self.stop_autopilot()
thread.join()
self.send_autopilot_msg(ap.Command(
ap.CommandOptions.CLEAR_WPS))
self.send_autopilot_msg(ap.AddWaypoints(wp_list))
self.switch_ap_mode(ap.GuidanceModeOptions.PATH_FOLLOWING)
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.YAW, False))
# if len(wp_list[0]) > 3:
# self.send_autopilot_msg(ap.TrackingSpeed(wp_list[0][3]))
# else:
self.send_autopilot_msg(
ap.TrackingSpeed(CollisionSettings.tracking_speed))
def get_pos(self):
if not self.ask_for_desired:
self.send_autopilot_msg(ap.GetMessage(ap.MsgType.ROV_STATE))
else:
self.send_autopilot_msg(
ap.GetMessage(
[ap.MsgType.ROV_STATE, ap.MsgType.ROV_STATE_DESIRED]))
def close(self):
if Settings.enable_autopilot:
if LosSettings.enable_los:
self.los_stop_event.set()
if CollisionSettings.send_new_wps and self.in_control:
logger.info('Shutting down autopilot')
self.ask_for_desired = True
thread = self.stop_autopilot()
thread.join()
logger.info(
'In stationkeeping mode. Waiting for small errors.')
state_error = None
self.ap_pos_received.clear()
while state_error is None or not state_error.is_small(
LosSettings.enable_los):
self.ap_pos_received.wait()
if self.cur_desired_pos_msg is None or self.cur_pos_msg is None:
self.ap_pos_received.clear()
continue
state_error = self.cur_pos_msg - self.cur_desired_pos_msg
# All errors are small
self.send_autopilot_msg(ap.RemoteControlRequest(False))
logger.info('Released control of autopilot')
if CollisionSettings.send_new_wps:
self.autopilot_watchdog_stop_event.set()
def set_speed(self, speed):
if speed != self.surge_speed:
self.surge_speed = speed
if Settings.input_source == 0:
self.msg_client.send_autopilot_msg(ap.CruiseSpeed(speed))
else:
self.msg_client.send_msg('vel_com', speed)
logger.info('Surge speed adjusted to {:.2f} m/s'.format(speed))
def stop_and_turn(self, theta):
if Settings.enable_autopilot:
logger.info('Stop and turn {:.2f} deg'.format(theta * 180.0 /
np.pi))
if LosSettings.enable_los:
if self.los_thread.isAlive():
self.los_stop_event.set()
self.los_thread.join()
self.los_stop_event.clear()
else:
self.stop_autopilot()
self.send_autopilot_msg(
ap.Setpoint(theta + self.cur_pos_msg.yaw, ap.Dofs.YAW, True))
def ping_autopilot_server(self):
# Get empty msg to keep control
if self.autopilot_sid == 0:
# self.send_autopilot_msg(ap.ControllerOptions([]))
self.send_autopilot_msg(ap.RemoteControlRequest(True))
logger.info("Asked for remote control")
return False
else:
self.in_control = True
if Settings.pos_msg_source == 1:
# self.send_autopilot_msg(ap.ControllerOptions([ap.Dofs.SURGE, ap.Dofs.SWAY, ap.Dofs.HEAVE, ap.Dofs.YAW]))
self.autopilot_watchdog_stop_event.set()
self.switch_ap_mode(ap.GuidanceModeOptions.STATION_KEEPING)
# while self.cur_pos_msg is None:
# pass
# self.send_autopilot_msg(ap.Setpoint(self.cur_pos_msg.lat, ap.Dofs.SURGE, True))
# self.send_autopilot_msg(ap.Setpoint(self.cur_pos_msg.long, ap.Dofs.SWAY, True))
# self.send_autopilot_msg(ap.Setpoint(self.cur_pos_msg.yaw, ap.Dofs.YAW, True))
# self.send_autopilot_msg(ap.VerticalPos(ap.VerticalPosOptions.ALTITUDE, 10))
else:
self.send_autopilot_msg(ap.GetMessage(
ap.MsgType.EMPTY_MESSAGE))
return True
def stop_autopilot(self):
if Settings.enable_autopilot:
logger.info('Stopping ROV')
if self.cur_pos_msg is None:
return
if self.guidance_mode is not ap.GuidanceModeOptions.STATION_KEEPING:
if self.guidance_mode is ap.GuidanceModeOptions.PATH_FOLLOWING:
# self.switch_ap_mode(ap.GuidanceModeOptions.PATH_FOLLOWING))
self.send_autopilot_msg(ap.TrackingSpeed(0))
if self.guidance_mode is ap.GuidanceModeOptions.CRUISE_MODE:
self.send_autopilot_msg(ap.CruiseSpeed(0))
counter = 0
n_set = 1
max = 20
surge = np.ones(max)
surge[0] = self.cur_pos_msg.v_surge
acc = 1
while abs(np.sum(surge) / n_set) > 0.02:
# print(np.sum(v_surge)/n_set, acc)
self.ap_pos_received.clear()
self.ap_pos_received.wait(0.1)
counter += 1
if counter == max:
counter = 0
surge[counter] = self.cur_pos_msg.v_surge
acc = abs((surge[counter] - surge[counter - 1]) / 0.1)
if n_set < max:
n_set += 1
self.switch_ap_mode(ap.GuidanceModeOptions.STATION_KEEPING)
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.SURGE, True))
# self.send_autopilot_msg(ap.Setpoint(0, ap.Dofs.SWAY, True))
# self.send_autopilot_msg(ap.Setpoint(self.cur_pos_msg.yaw, ap.Dofs.YAW, True))
self.send_autopilot_msg(
ap.VerticalPos(ap.VerticalPosOptions.ALTITUDE,
self.cur_pos_msg.alt))
else:
return
def switch_ap_mode(self, mode):
if Settings.enable_autopilot:
if self.guidance_mode is not mode:
self.guidance_mode = mode
logger.info('Switching to {}'.format(mode.name))
self.send_autopilot_msg(ap.GuidanceMode(mode))
def handler_factory(callback):
def createHandler(*args, **keys):
return Handler(callback, *args, **keys)
return createHandler
if __name__ == '__main__':
from time import sleep
from settings import ConnectionSettings
client = UdpClient(ConnectionSettings.sonar_port,
ConnectionSettings.pos_port,
ConnectionSettings.autopilot_ip,
ConnectionSettings.autopilot_server_port,
ConnectionSettings.autopilot_listen_port)
client.start()
while not client.in_control:
pass
client.switch_ap_mode(ap.GuidanceModeOptions.CRUISE_MODE)
client.send_autopilot_msg(ap.CruiseSpeed(0.2))
# counter = 0
# while counter < 4:
# sleep(5)
# client.send_autopilot_msg(ap.Setpoint(counter*np.pi/4, ap.Dofs.YAW, True))
# counter += 1
sleep(5)
print('closing down')
client.close()
print('closed')
def loop(self):
logger.info('New path received')
if len(self.wp_list) < 2 or self.pos_msg is None:
logger.info('To few waypoints. Returning to Stationkeeping.')
self.msg_client.stop_autopilot()
return
if LosSettings.log_paths:
chi_log = []
surge_log = []
timestamp = []
delta_log = []
e_log = []
wp_change = []
roa_log = []
slow_down_dist_log = []
slown_down_vel_log = []
pos_log = []
# self.surge_speed = 0
# Initial check
pos_msg, wp_list, wp_counter, wp_grad, segment_lengths = self.get_info(
)
chi, delta, e = self.get_los_values(wp_list[0], wp_list[1], pos_msg)
start_chi = wrapToPi(get_angle(wp_list[0], wp_list[1]))
logger.info('Start Chi: {:.2f}'.format(start_chi * 180.0 / np.pi))
# TODO: CHeck if on initial path
if (segment_length(wp_list[0], (pos_msg.north, pos_msg.east)) > LosSettings.roa or
abs(start_chi - pos_msg.yaw) > LosSettings.start_heading_diff) and \
not (segment_lengths[0] - delta > 0 and abs(e) < LosSettings.roa):
logger.info(
'Not on path, s: {:.2f}, e: {:.2f}, d_psi: {:.2f}'.format(
segment_lengths[0] - delta, e,
(start_chi - pos_msg.yaw) * 180.0 / np.pi))
# Wait for low speed before stationkeeping
# logger.info('Speed to high to start LOS-guidance')
# self.set_speed(0)
# while self.start_event.wait() and not self.stopped_event.isSet():
# pos_msg = self.get_pos()
# if pos_msg.v_surge < 0.01:
# break
th = self.msg_client.stop_autopilot()
th.join()
# self.msg_client.switch_ap_mode(ap.GuidanceModeOptions.STATION_KEEPING)
if Settings.input_source == 0:
self.msg_client.send_autopilot_msg(
ap.Setpoint(wp_list[0][0], ap.Dofs.NORTH, True))
self.msg_client.send_autopilot_msg(
ap.Setpoint(wp_list[0][1], ap.Dofs.EAST, True))
self.msg_client.send_autopilot_msg(
ap.Setpoint(wp_list[0][2], ap.Dofs.DEPTH, True))
self.msg_client.send_autopilot_msg(
ap.Setpoint(start_chi, ap.Dofs.YAW, True))
else:
self.msg_client.send_msg('north_com', wp_list[0][0])
self.msg_client.send_msg('east_com', wp_list[0][1])
self.msg_client.send_msg('depth_com', wp_list[0][2])
self.msg_client.send_msg('yaw_com', start_chi)
# logger.info('To far from inital setpoint. Moving to: '
# '[N={:.6f}, E={:.6f}, D={:.2f}, YAW={:.2f} deg]'.format(wp_list[0][0], wp_list[0][1],
# wp_list[0][2], start_chi*180.0/np.pi))
logger.info(
'To far from inital setpoint. Yaw_hat:{:.2f}, north_hat: {:.2f}, east_hat: {:.2f}'
.format((start_chi - pos_msg.yaw) * 180.0 / np.pi,
wp_list[0][0] - pos_msg.north,
wp_list[0][1] - pos_msg.east))
while self.start_event.wait() and not self.stopped_event.isSet() and \
(segment_length(wp_list[0], (pos_msg.north, pos_msg.east)) > 1 or
abs(start_chi - pos_msg.yaw) > LosSettings.start_heading_diff):
pos_msg = self.get_pos()
if Settings.input_source == 0:
self.msg_client.switch_ap_mode(ap.GuidanceModeOptions.CRUISE_MODE)
# self.set_speed(self.normal_surge_speed)
turn_speed, slow_down_dist = self.turn_vel(0)
if segment_lengths[wp_counter] < LosSettings.roa:
self.roa = segment_lengths[wp_counter] / 2
else:
self.roa = LosSettings.roa
if LosSettings.log_paths:
roa_log.append(self.roa)
slow_down_dist_log.append(slow_down_dist)
slown_down_vel_log.append(turn_speed)
# logger.info('Setting cruise speed: {} m/s'.format(self.surge_speed))
if Settings.input_source == 0:
self.msg_client.send_autopilot_msg(
ap.Setpoint(wp_list[0][2], ap.Dofs.DEPTH, True))
self.msg_client.send_autopilot_msg(
ap.Setpoint(chi, ap.Dofs.YAW, True))
else:
self.msg_client.send_msg('depth_com', wp_list[0][2])
self.msg_client.send_msg('yaw_com', wrapToPi(chi))
if (slow_down_dist < 0) or (delta <= slow_down_dist):
self.set_speed(turn_speed)
else:
self.set_speed(self.normal_surge_speed)
if LosSettings.log_paths:
chi_log.append(chi)
surge_log.append(self.surge_speed)
timestamp.append(datetime.now().strftime("%H:%M:%S"))
delta_log.append(delta)
e_log.append(e)
wp_change.append(datetime.now().strftime("%H:%M:%S"))
pos_log.append(pos_msg.to_tuple())
logger.info('Path Started')
while not self.stopped_event.isSet() and self.start_event.wait(
) and self.wp_counter < len(self.wp_list):
pos_msg = self.get_pos()
# Get new setpoint
chi, delta, e = self.get_los_values(wp_list[wp_counter],
wp_list[wp_counter + 1],
pos_msg)
# # Check if slow down
# if slow_down_dist >= 0 and delta <= slow_down_dist:
# self.set_speed(turn_speed)
# Check if ROA
if delta < self.roa:
wp_counter += 1
if LosSettings.log_paths:
wp_change.append(datetime.now().strftime("%H:%M:%S"))
try:
turn_speed, slow_down_dist = self.turn_vel(wp_counter)
if segment_lengths[wp_counter] < LosSettings.roa:
self.roa = segment_lengths[wp_counter] / 2
else:
self.roa = LosSettings.roa
if LosSettings.log_paths:
roa_log.append(self.roa)
slow_down_dist_log.append(slow_down_dist)
slown_down_vel_log.append(turn_speed)
if Settings.input_source == 0:
self.msg_client.send_autopilot_msg(
ap.Setpoint(wp_list[wp_counter + 1][2],
ap.Dofs.DEPTH, True))
else:
self.msg_client.send_msg('alt_com', 2.0)
chi, delta, e = self.get_los_values(
wp_list[wp_counter], wp_list[wp_counter + 1], pos_msg)
except IndexError:
logger.info('Final Waypoint reached!')
break
logger.info(
'Current wp is WP{}: [N={:.6f}, E={:.6f}, D={:.2f}], ROA={:.2f}'
.format(wp_counter, wp_list[wp_counter][0],
wp_list[wp_counter][1], wp_list[wp_counter][2],
self.roa))
# Check if slow down
if (slow_down_dist < 0) or (delta <= slow_down_dist):
self.set_speed(turn_speed)
else:
self.set_speed(self.normal_surge_speed)
# Send new setpoints
if abs(chi - self.last_chi) > LosSettings.send_new_heading_limit:
# logger.info('Chi: {:2f}'.format(wrapToPi(chi)*180.0/np.pi))
if Settings.input_source == 0:
self.msg_client.send_autopilot_msg(
ap.Setpoint(wrapToPi(chi), ap.Dofs.YAW, True))
else:
self.msg_client.send_msg('yaw_com', wrapToPi(chi))
self.last_chi = chi
if LosSettings.log_paths:
chi_log.append(chi)
surge_log.append(self.surge_speed)
timestamp.append(datetime.now().strftime("%H:%M:%S"))
delta_log.append(delta)
e_log.append(e)
pos_log.append(pos_msg.to_tuple())
with self.lock:
self.wp_counter = wp_counter
self.e = e
self.delta = delta
with self.lock:
self.e = 0
self.delta = 0
if not self.restart_event.isSet():
logger.info('WP loop finished: Stopping ROV')
self.msg_client.stop_autopilot()
else:
logger.info('WP loop finished: Restarting loop')
if LosSettings.log_paths:
savemat('{}Los_log_{}'.format(
Settings.log_folder,
datetime.now().strftime("%Y%m%d-%H%M%S")),
mdict={
'chi': np.array(chi_log),
'surge': np.array(surge_log),
'time': timestamp,
'path': wp_list,
'delta': np.array(delta_log),
'cross_track': np.array(e_log),
'slow_down_dist': np.array(slow_down_dist_log),
'roa': np.array(roa_log),
'slow_down_vel': np.array(slown_down_vel_log),
'wp_change': wp_change,
'pos': np.array(pos_log)
})
