diff --git a/docs/agents/acu_agent.rst b/docs/agents/acu_agent.rst index c08f3cbeb..c02faa020 100644 --- a/docs/agents/acu_agent.rst +++ b/docs/agents/acu_agent.rst @@ -26,6 +26,12 @@ installed to use this Agent. This can be installed via: $ pip install 'soaculib @ git+https://github.com/simonsobs/soaculib.git@master' +Additionally, ``socs`` should be installed with the ``acu`` group: + +.. code-block:: bash + + $ pip install -U socs[acu] + Configuration File Examples --------------------------- Below are configuration examples for the ocs config file and for soaculib. @@ -88,6 +94,61 @@ example configuration block is below:: } +Sun Avoidance +------------- + +The Sun's position, and the potential danger of the Sun to the +equipment, is monitored and reported by the ``monitor_sun`` Process. +If enabled to do so, this Process can trigger the ``escape_sun_now`` +Task, which will cause the platform to move to a Sun-safe position. + +The parameters used by an Agent instance for Sun Avoidance are +determined like this: + +- Default parameters for each platform (LAT and SATP) are in the Agent + code. +- On start-up the default parameters for platform are modified + according to any command-line parameters passed in by the user. +- Some parameters can be altered using the command line. + +The avoidance policy is defined by a few key parameters and concepts; +please see the descriptions of ``sun_dist``, ``sun_time``, +``exclusion_radius``, and more in the :mod:`socs.agents.acu.avoidance` +module documentation. + +The ``exclusion_radius`` can be configured from the Agent command +line, and also through the ``update_sun`` Task. + +When Sun Avoidance is active (``active_avoidance`` is ``True``), the +following will be enforced: + +- When a user initiates the ``go_to`` Task, the target point of the + motion will be checked. If it is not Sun-safe, the Task will exit + immediately with an error. If the Task cannot find a set of moves + that are Sun-safe and that do not violate other requirements + (azimuth and elevation limits; the ``el_dodging`` policy), then the + Task will exit with error. The move may be executed as a series of + separate legs (e.g. the Task may move first to an intermediate + elevation, then slew in azimuth, then continue to the final + elevation) rather than simulataneously commanding az and el motion. +- When a user starts the ``generate_scan`` Process, the sweep of the + scan will be checked for Sun-safety, and the Process will exit with + error if it is not. Furthermore, any movement required prior to + starting the scan will be checked in the same way as for the + ``go_to`` Task. +- If the platform, at any time, enters a position that is not + Sun-safe, then an Escape will be Initiated. During an Escape, any + running ``go_to`` or ``generate_scan`` operations will be cancelled, + and further motions are blocked. The platform will be driven to a + position at due North or due South. The current elevation of the + platform will be preserved, unless that is not Sun-safe (in which + case lower elevations will be attempted). The Escape feature is + active, even when motions are not in progress, as long as the + ``monitor_sun`` Process is running. However -- the Escape operation + requires that the platform be in Remote operation mode, with no + persistent faults. + + Exercisor Mode -------------- @@ -149,5 +210,14 @@ acquisition processes are running:: Supporting APIs --------------- +drivers (Scanning support) +`````````````````````````` + .. automodule:: socs.agents.acu.drivers :members: + +avoidance (Sun Avoidance) +````````````````````````` + +.. automodule:: socs.agents.acu.avoidance + :members: diff --git a/docs/user/installation.rst b/docs/user/installation.rst index 4cd0114a1..ce1f0f580 100644 --- a/docs/user/installation.rst +++ b/docs/user/installation.rst @@ -22,6 +22,8 @@ The different groups, and the agents they provide dependencies for are: - Supporting Agents * - ``all`` - All Agents + * - ``acu`` + - ACU Agent * - ``labjack`` - Labjack Agent * - ``magpie`` diff --git a/requirements.txt b/requirements.txt index 6c02ae1dd..1f74fe79b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -16,6 +16,8 @@ pyyaml # acu agent soaculib @ git+https://github.com/simonsobs/soaculib.git@master +so3g +pixell # holography agent - python 3.8 only! # -r requirements/holography.txt diff --git a/setup.py b/setup.py index ea348ef17..37b6f2274 100644 --- a/setup.py +++ b/setup.py @@ -7,7 +7,11 @@ # Optional Dependencies # ACU Agent -# acu_deps = ['soaculib @ git+https://github.com/simonsobs/soaculib.git@master'] +acu_deps = [ + # 'soaculib @ git+https://github.com/simonsobs/soaculib.git@master', + 'pixell', + 'so3g', +] # Holography FPGA and Synthesizer Agents # holography_deps = [ # Note: supports python 3.8 only! @@ -53,10 +57,9 @@ # 'xy_stage_control @ git+https://github.com/kmharrington/xy_stage_control.git@main', # ] -# Note: Not including the holograph deps, which are Python 3.8 only -# all_deps = acu_deps + labjack_deps + magpie_deps + pfeiffer_deps + \ -# pysmurf_deps + smurf_sim_deps + synacc_deps + xy_stage_deps -all_deps = labjack_deps + magpie_deps + pfeiffer_deps + \ +# Note: Not including the holograph deps, which are Python 3.8 only. Also not +# including any dependencies with only direct references. +all_deps = acu_deps + labjack_deps + magpie_deps + pfeiffer_deps + \ smurf_sim_deps + synacc_deps + timing_master_deps all_deps = list(set(all_deps)) @@ -111,7 +114,7 @@ ], extras_require={ 'all': all_deps, - # 'acu': acu_deps, + 'acu': acu_deps, # 'holography': holography_deps, 'labjack': labjack_deps, 'magpie': magpie_deps, diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py index e3fab147c..5ed66a231 100644 --- a/socs/agents/acu/agent.py +++ b/socs/agents/acu/agent.py @@ -13,9 +13,10 @@ from ocs import ocs_agent, site_config from ocs.ocs_twisted import TimeoutLock from soaculib.twisted_backend import TwistedHttpBackend -from twisted.internet import protocol, reactor +from twisted.internet import protocol, reactor, threads from twisted.internet.defer import DeferredList, inlineCallbacks +from socs.agents.acu import avoidance from socs.agents.acu import drivers as sh from socs.agents.acu import exercisor @@ -39,6 +40,27 @@ } +#: Default Sun avoidance params by platform type (enabled, policy) +SUN_CONFIGS = { + 'ccat': { + 'enabled': False, + 'policy': {}, + }, + 'satp': { + 'enabled': True, + 'policy': { + 'exclusion_radius': 20, + 'el_horizon': 10, + 'min_sun_time': 1800, + 'response_time': 7200, + }, + }, +} + +#: How often to refresh to Sun Safety map (valid up to 2x this time) +SUN_MAP_REFRESH = 6 * avoidance.HOUR + + class ACUAgent: """Agent to acquire data from an ACU and control telescope pointing with the ACU. @@ -62,11 +84,24 @@ class ACUAgent: list should be drawn from "az", "el", and "third". disable_idle_reset (bool): If True, don't auto-start idle_reset process for LAT. + min_el (float): If not None, override the default configured + elevation lower limit. + max_el (float): If not None, override the default configured + elevation upper limit. + avoid_sun (bool): If set, override the default Sun + avoidance setting (i.e. force enable or disable the feature). + fov_radius (float): If set, override the default Sun + avoidance radius (i.e. the radius of the field of view, in + degrees, to use for Sun avoidance purposes). """ def __init__(self, agent, acu_config='guess', exercise_plan=None, - startup=False, ignore_axes=None, disable_idle_reset=False): + startup=False, ignore_axes=None, disable_idle_reset=False, + min_el=None, max_el=None, + avoid_sun=None, fov_radius=None): + self.log = agent.log + # Separate locks for exclusive access to az/el, and boresight motions. self.azel_lock = TimeoutLock() self.boresight_lock = TimeoutLock() @@ -85,6 +120,13 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None, self.monitor_fields = status_keys.status_fields[self.acu_config['platform']]['status_fields'] self.motion_limits = self.acu_config['motion_limits'] + if min_el: + self.log.warn(f'Override: min_el={min_el}') + self.motion_limits['elevation']['lower'] = min_el + if max_el: + self.log.warn(f'Override: max_el={max_el}') + self.motion_limits['elevation']['upper'] = max_el + # This initializes self.scan_params; these become the default # scan params when calling generate_scan. They can be changed # during run time; they can also be overridden when calling @@ -102,9 +144,10 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None, if len(self.ignore_axes): agent.log.warn('User requested ignore_axes={i}', i=self.ignore_axes) - self.exercise_plan = exercise_plan + self._reset_sun_params(enabled=avoid_sun, + radius=fov_radius) - self.log = agent.log + self.exercise_plan = exercise_plan # self.data provides a place to reference data from the monitors. # 'status' is populated by the monitor operation @@ -148,6 +191,11 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None, self._simple_process_stop, blocking=False, startup=startup) + agent.register_process('monitor_sun', + self.monitor_sun, + self._simple_process_stop, + blocking=False, + startup=startup) agent.register_process('generate_scan', self.generate_scan, self._simple_process_stop, @@ -216,6 +264,13 @@ def __init__(self, agent, acu_config='guess', exercise_plan=None, agent.register_task('clear_faults', self.clear_faults, blocking=False) + agent.register_task('update_sun', + self.update_sun, + blocking=False) + agent.register_task('escape_sun_now', + self.escape_sun_now, + blocking=False, + aborter=self._simple_task_abort) # Automatic exercise program... if exercise_plan: @@ -585,7 +640,9 @@ def monitor(self, session, params): del new_influx_blocks[k] for block in new_influx_blocks.values(): - self.agent.publish_to_feed('acu_status_influx', block) + # Check that we have data (commands and corotator often don't) + if len(block['data']) > 0: + self.agent.publish_to_feed('acu_status_influx', block) influx_blocks.update(new_influx_blocks) # Assemble data for aggregator ... @@ -1083,7 +1140,8 @@ def get_history(t): return success, msg @inlineCallbacks - def _go_to_axes(self, session, el=None, az=None, third=None): + def _go_to_axes(self, session, el=None, az=None, third=None, + clear_faults=False): """Execute a movement along multiple axes, using "Preset" mode. This just launches _go_to_axis on each required axis, and collects the results. @@ -1093,6 +1151,7 @@ def _go_to_axes(self, session, el=None, az=None, third=None): az (float): target for Azimuth axis (ignored if None). el (float): target for Elevation axis (ignored if None). third (float): target for Boresight axis (ignored if None). + clear_faults (bool): whether to clear ACU faults first. Returns: ok (bool): True if all motions completed successfully and @@ -1113,6 +1172,10 @@ def _go_to_axes(self, session, el=None, az=None, third=None): if len(move_defs) is None: return True, 'No motion requested.' + if clear_faults: + yield self.acu_control.clear_faults() + yield dsleep(1) + moves = yield DeferredList([d for n, d in move_defs]) all_ok, msgs = True, [] for _ok, result in moves: @@ -1151,6 +1214,9 @@ def go_to(self, session, params): if not acquired: return False, f"Operation failed: {self.azel_lock.job} is running." + if self._get_sun_policy('motion_blocked'): + return False, "Motion blocked; Sun avoidance in progress." + self.log.info('Clearing faults to prepare for motion.') yield self.acu_control.clear_faults() yield dsleep(1) @@ -1169,10 +1235,22 @@ def go_to(self, session, params): f'{axis}={target} not in accepted range, ' f'[{limits[0]}, {limits[1]}].') - self.log.info(f'Commanded position: az={target_az}, el={target_el}') + self.log.info(f'Requested position: az={target_az}, el={target_el}') session.set_status('running') - all_ok, msg = yield self._go_to_axes(session, az=target_az, el=target_el) + legs, msg = yield self._get_sunsafe_moves(target_az, target_el) + if msg is not None: + self.log.error(msg) + return False, msg + + if len(legs) > 1: + self.log.info(f'Executing move via {len(legs)} separate legs (sun optimized)') + + for leg_az, leg_el in legs: + all_ok, msg = yield self._go_to_axes(session, az=leg_az, el=leg_el) + if not all_ok: + break + if all_ok and params['end_stop']: yield self._set_modes(az='Stop', el='Stop') @@ -1477,6 +1555,9 @@ def generate_scan(self, session, params): Process .stop method is called).. """ + if self._get_sun_policy('motion_blocked'): + return False, "Motion blocked; Sun avoidance in progress." + self.log.info('User scan params: {params}', params=params) az_endpoint1 = params['az_endpoint1'] @@ -1539,6 +1620,15 @@ def generate_scan(self, session, params): self.log.info('The plan: {plan}', plan=plan) self.log.info('The scan_params: {scan_params}', scan_params=scan_params) + # Before any motion, check for sun safety. + ok, msg = self._check_scan_sunsafe(az_endpoint1, az_endpoint2, el_endpoint1, + az_speed, az_accel) + if ok: + self.log.info('Sun safety check passes: {msg}', msg=msg) + else: + self.log.error('Sun safety check fails: {msg}', msg=msg) + return False, 'Scan is not Sun Safe.' + # Clear faults. self.log.info('Clearing faults to prepare for motion.') yield self.acu_control.clear_faults() @@ -1551,10 +1641,14 @@ def generate_scan(self, session, params): # Seek to starting position self.log.info(f'Moving to start position, az={plan["init_az"]}, el={init_el}') - ok, msg = yield self._go_to_axes(session, az=plan['init_az'], el=init_el) - - if not ok: - return False, f'Start position seek failed with message: {msg}' + legs, msg = yield self._get_sunsafe_moves(plan['init_az'], init_el) + if msg is not None: + self.log.error(msg) + return False, msg + for leg_az, leg_el in legs: + ok, msg = yield self._go_to_axes(session, az=leg_az, el=leg_el) + if not ok: + return False, f'Start position seek failed with message: {msg}' # Prepare the point generator. g = sh.generate_constant_velocity_scan(az_endpoint1=az_endpoint1, @@ -1717,6 +1811,498 @@ def _run_track(self, session, point_gen, step_time, azonly=False, return False, 'Problems during scan' return True, 'Scan ended cleanly' + # + # Sun Safety Monitoring and Active Avoidance + # + + def _reset_sun_params(self, enabled=None, radius=None): + """Resets self.sun_params based on defaults for this platform. Note + if enabled or radius are specified here, they update the + defaults (so they endure for the life of the agent). + + """ + config = SUN_CONFIGS[self.acu_config['platform']] + + # These params update config for the entire run of agent. + if enabled is not None: + config['enabled'] = bool(enabled) + if radius is not None: + config['policy']['exclusion_radius'] = radius + + _p = { + # Global enable (but see "disable_until"). + 'active_avoidance': False, + + # Can be set to a timestamp, in which case Sun Avoidance + # is disabled until that time has passed. + 'disable_until': 0, + + # Flag for indicating normal motions should be blocked + # (Sun Escape is active). + 'block_motion': False, + + # Flag for update_sun to indicate Sun map needs recomputed + 'recompute_req': False, + + # If set, should be a timestamp at which escape_sun_now + # will be initiated. + 'next_drill': None, + + # Parameters for the Sun Safety Map computation. + 'safety_map_kw': { + 'sun_time_shift': 0, + }, + + # Avoidance policy, for use in avoidance decisions. + 'policy': None, + } + + # Populate default policy based on platform. + _p['active_avoidance'] = config['enabled'] + _p['policy'] = config['policy'] + + # And add in platform limits + _p['policy'].update({ + 'min_az': self.motion_limits['azimuth']['lower'], + 'max_az': self.motion_limits['azimuth']['upper'], + 'min_el': self.motion_limits['elevation']['lower'], + 'max_el': self.motion_limits['elevation']['upper'], + }) + + self.sun_params = _p + + def _get_sun_policy(self, key): + now = time.time() + p = self.sun_params + active = (p['active_avoidance'] and (now >= p['disable_until'])) + + if key == 'motion_blocked': + return active and p['block_motion'] + elif key == 'sunsafe_moves': + return active + elif key == 'escape_enabled': + return active + elif key == 'map_valid': + return (self.sun is not None + and self.sun.base_time is not None + and self.sun.base_time <= now + and self.sun.base_time >= now - 2 * SUN_MAP_REFRESH) + else: + return p[key] + + @ocs_agent.param('_') + @inlineCallbacks + def monitor_sun(self, session, params): + """monitor_sun() + + **Process** - Monitors and reports the position of the Sun; + maintains a Sun Safety Map for verifying that moves and scans + are Sun-safe; triggers a "Sun escape" if the boresight enters + an unsafe position. + + The monitoring functions are always active (as long as this + process is running). But the escape functionality must be + explicitly enabled (through the default platform + configuration, command line arguments, or the update_sun + task). + + Session data looks like this:: + + { + "timestamp": 1698848292.5579932, + "active_avoidance": false, + "disable_until": 0, + "block_motion": false, + "recompute_req": false, + "next_drill": null, + "safety_map_kw": { + "sun_time_shift": 0 + }, + "policy": { + "exclusion_radius": 20, + "el_horizon": 10, + "min_sun_time": 1800, + "response_time": 7200, + "min_az": -90, + "max_az": 450, + "min_el": 18.5, + "max_el": 90 + }, + "sun_pos": { + "map_exists": true, + "map_is_old": false, + "map_ref_time": 1698848179.1123455, + "platform_azel": [ + 90.0158, + 20.0022 + ], + "sun_radec": [ + 216.50815789438036, + -14.461844389380719 + ], + "sun_azel": [ + 78.24269024936028, + 60.919554369324096 + ], + "sun_dist": 41.75087242151837, + "sun_safe_time": 71760 + }, + "avoidance": { + "safety_unknown": false, + "warning_zone": false, + "danger_zone": false, + "escape_triggered": false, + "escape_active": false, + "last_escape_time": 0, + "sun_is_real": true + } + } + + In debugging, the Sun position might be falsified. In that + case the "sun_pos" subtree will contain an entry like this:: + + "WARNING": "Fake Sun Position is in use!", + + and "avoidance": "sun_is_real" will be set to false. (No + other functionality is changed when using a falsified Sun + position; flags are computed and actions decided based on the + false position.) + + """ + def _get_sun_map(): + # To run in thread ... + start = time.time() + new_sun = avoidance.SunTracker(policy=self.sun_params['policy'], + **self.sun_params['safety_map_kw']) + return new_sun, time.time() - start + + def _notify_recomputed(result): + nonlocal req_out + new_sun, compute_time = result + self.log.info('(Re-)computed Sun Safety Map (took %.1fs)' % + compute_time) + self.sun = new_sun + req_out = False + + req_out = False + self.sun = None + last_panic = 0 + + session.data = {} + session.set_status('running') + + while session.status in ['starting', 'running']: + new_data = { + 'timestamp': time.time(), + } + new_data.update(self.sun_params) + + try: + az, el = [self.data['status']['summary'][f'{ax}_current_position'] + for ax in ['Azimuth', 'Elevation']] + if az is None or el is None: + raise KeyError + except KeyError: + az, el = None, None + + no_map = self.sun is None + old_map = (not no_map + and self.sun._now() - self.sun.base_time > SUN_MAP_REFRESH) + do_recompute = ( + not req_out + and (no_map or old_map or self.sun_params['recompute_req']) + ) + + if do_recompute: + req_out = True + self.sun_params['recompute_req'] = False + threads.deferToThread(_get_sun_map).addCallback( + _notify_recomputed) + + new_data.update({ + 'sun_pos': { + 'map_exists': not no_map, + 'map_is_old': old_map, + 'map_ref_time': None if no_map else self.sun.base_time, + 'platform_azel': (az, el), + }, + }) + + sun_is_real = True # flags time shift during debugging. + if self.sun is not None: + info = self.sun.get_sun_pos(az, el) + sun_is_real = ('WARNING' not in info) + new_data['sun_pos'].update(info) + if az is not None: + t = self.sun.check_trajectory([az], [el])['sun_time'] + new_data['sun_pos']['sun_safe_time'] = t if t > 0 else 0 + + # Are we currently in safe position? + safety_known, danger_zone, warning_zone = False, False, False + if self.sun is not None: + safety_known = True + danger_zone = (t < self.sun_params['policy']['min_sun_time']) + warning_zone = (t < self.sun_params['policy']['response_time']) + + # Has a drill been requested? + drill_req = (self.sun_params['next_drill'] is not None + and self.sun_params['next_drill'] <= time.time()) + + # Should we be doing a escape_sun_now? + panic_for_real = safety_known and danger_zone and self._get_sun_policy('escape_enabled') + panic_for_fun = drill_req + + # Is escape_sun_now task running? + ok, msg, _session = self.agent.status('escape_sun_now') + escape_in_progress = (_session.get('status', 'done') != 'done') + + # Block motion as long as we are not sun-safe. + self.sun_params['block_motion'] = (panic_for_real or escape_in_progress) + + new_data['avoidance'] = { + 'safety_unknown': not safety_known, + 'warning_zone': warning_zone, + 'danger_zone': danger_zone, + 'escape_triggered': panic_for_real, + 'escape_active': escape_in_progress, + 'last_escape_time': last_panic, + 'sun_is_real': sun_is_real, + } + + if (panic_for_real or panic_for_fun) and (time.time() - last_panic > 60.): + self.log.warn('monitor_sun is requesting escape_sun_now.') + self.sun_params['next_drill'] = None + self.agent.start('escape_sun_now') + last_panic = time.time() + + # Update session. + session.data.update(new_data) + + yield dsleep(1) + + @ocs_agent.param('reset', type=bool, default=None) + @ocs_agent.param('enable', type=bool, default=None) + @ocs_agent.param('temporary_disable', type=float, default=None) + @ocs_agent.param('escape', type=bool, default=None) + @ocs_agent.param('avoidance_radius', type=float, default=None) + @ocs_agent.param('shift_sun_hours', type=float, default=None) + def update_sun(self, session, params): + """update_sun(reset, enable, temporary_disable, escape, \ + avoidance_radius, shift_sun_hours) + + **Task** - Update Sun monitoring and avoidance parameters. + + Args: + + reset (bool): If True, reset all sun_params to the platform + defaults. (The "defaults" includes any overrides + specified on Agent command line.) + enable (bool): If True, enable active Sun avoidance. If + avoidance was temporarily disabled it is re-enabled. If + False, disable active Sun avoidance (non-temporarily). + temporary_disable (float): If set, disable Sun avoidance for + this number of seconds. + escape (bool): If True, schedule an escape drill for 10 + seconds from now. + avoidance_radius (float): If set, change the FOV radius + (degrees), for Sun avoidance purposes, to this number. + shift_sun_hours (float): If set, compute the Sun position as + though it were this many hours in the future. This is for + debugging, testing, and work-arounds. Pass zero to + cancel. + + """ + do_recompute = False + now = time.time() + self.log.info('update_sun params: {params}', + params={k: v for k, v in params.items() + if v is not None}) + + if params['reset']: + self._reset_sun_params() + do_recompute = True + if params['enable'] is not None: + self.sun_params['active_avoidance'] = params['enable'] + self.sun_params['disable_until'] = 0 + if params['temporary_disable'] is not None: + self.sun_params['disable_until'] = params['temporary_disable'] + now + if params['escape']: + self.log.warn('Setting sun escape drill to start in 10 seconds.') + self.sun_params['next_drill'] = now + 10 + if params['avoidance_radius'] is not None: + self.sun_params['policy']['exclusion_radius'] = \ + params['avoidance_radius'] + do_recompute = True + if params['shift_sun_hours'] is not None: + self.sun_params['safety_map_kw']['sun_time_shift'] = \ + params['shift_sun_hours'] * 3600 + do_recompute = True + + if do_recompute: + self.sun_params['recompute_req'] = True + + return True, 'Params updated.' + + @ocs_agent.param('_') + @inlineCallbacks + def escape_sun_now(self, session, params): + """escape_sun_now() + + **Task** - Take control of the platform, and move it to a + Sun-Safe position. This will abort/stop any current go_to or + generate_scan, identify the safest possible path to North or + South (without changing elevation, if possible), and perform + the moves to get there. + + """ + state = 'init' + last_state = state + + session.data = {'state': state, + 'timestamp': time.time()} + session.set_status('running') + + while session.status in ['starting', 'running'] and state not in ['escape-done']: + az, el = [self.data['status']['summary'][f'{ax}_current_position'] + for ax in ['Azimuth', 'Elevation']] + + if state == 'init': + state = 'escape-abort' + elif state == 'escape-abort': + # raise stop flags and issue stop on motion ops + for op in ['generate_scan', 'go_to']: + self.agent.stop(op) + self.agent.abort(op) + state = 'escape-wait-idle' + timeout = 30 + elif state == 'escape-wait-idle': + for op in ['generate_scan', 'go_to']: + ok, msg, _session = self.agent.status(op) + if _session.get('status', 'done') != 'done': + break + else: + state = 'escape-move' + last_move = time.time() + timeout -= 1 + if timeout < 0: + state = 'escape-stop' + elif state == 'escape-stop': + yield self._stop() + state = 'escape-move' + last_move = time.time() + elif state == 'escape-move': + self.log.info('Getting escape path for (t, az, el) = ' + '(%.1f, %.3f, %.3f)' % (time.time(), az, el)) + escape_path = self.sun.find_escape_paths(az, el) + if escape_path is None: + self.log.error('Failed to find acceptable path; using ' + 'failsafe (South, low el).') + legs = [(180., max(self.sun_params['policy']['min_el'], 0))] + else: + legs = escape_path['moves'].nodes[1:] + self.log.info('Escaping to (az, el)={pos} ({n} moves)', + pos=legs[-1], n=len(legs)) + state = 'escape-move-legs' + leg_d = None + elif state == 'escape-move-legs': + def _leg_done(result): + nonlocal state, last_move, leg_d + all_ok, msg = result + if not all_ok: + self.log.error('Leg failed.') + # Recompute the escape path. + if time.time() - last_move > 60: + self.log.error('Too many failures -- giving up for now') + state = 'escape-done' + else: + state = 'escape-move' + else: + leg_d = None + last_move = time.time() + if not self._get_sun_policy('escape_enabled'): + state = 'escape-done' + if leg_d is None: + if len(legs) == 0: + state = 'escape-done' + else: + leg_az, leg_el = legs.pop(0) + leg_d = self._go_to_axes(session, az=leg_az, el=leg_el, + clear_faults=True) + leg_d.addCallback(_leg_done) + elif state == 'escape-done': + # This block won't run -- loop will exit. + pass + + session.data['state'] = state + if state != last_state: + self.log.info('escape_sun_now: state is now "{state}"', state=state) + last_state = state + yield dsleep(1) + + return True, "Exited." + + def _check_scan_sunsafe(self, az1, az2, el, v_az, a_az): + # Include a bit of buffer for turn-arounds. + az1, az2 = min(az1, az2), max(az1, az2) + turn = v_az**2 / a_az + az1 -= turn + az2 += turn + n = max(2, int(np.ceil((az2 - az1) / 1.))) + azs = np.linspace(az1, az2, n) + + info = self.sun.check_trajectory(azs, azs * 0 + el) + safe = info['sun_time'] >= self.sun_params['policy']['min_sun_time'] + if safe: + msg = 'Scan is safe for %.1f hours' % (info['sun_time'] / 3600) + else: + msg = 'Scan will be unsafe in %.1f hours' % (info['sun_time'] / 3600) + + if self._get_sun_policy('sunsafe_moves'): + return safe, msg + else: + return True, 'Sun-safety not active; %s' % msg + + def _get_sunsafe_moves(self, target_az, target_el): + """Given a target position, find a Sun-safe way to get there. This + will either be a direct move, or else an ordered slew in az + before el (or vice versa). + + Returns (legs, msg). If legs is None, it indicates that no + Sun-safe path could be found; msg is an error message. If a + path can be found, the legs is a list of intermediate move + targets, ``[(az0, el0), (az1, el1) ...]``, terminating on + ``(target_az, target_el)``. msg is None in that case. + + When Sun avoidance is not enabled, this function returns as + though the direct path to the target is a safe one. + + """ + if not self._get_sun_policy('sunsafe_moves'): + return [(target_az, target_el)], None + + if not self._get_sun_policy('map_valid'): + return None, 'Sun Safety Map not computed or stale; run the monitor_sun process.' + + # Check the target position and block it outright. + if self.sun.check_trajectory([target_az], [target_el])['sun_time'] <= 0: + return None, 'Requested target position is not Sun-Safe.' + + # Ok, so where are we now ... + try: + az, el = [self.data['status']['summary'][f'{ax}_current_position'] + for ax in ['Azimuth', 'Elevation']] + if az is None or el is None: + raise KeyError + except KeyError: + return None, 'Current position could not be determined.' + + moves = self.sun.analyze_paths(az, el, target_az, target_el) + move, decisions = self.sun.select_move(moves) + if move is None: + return None, 'No Sun-Safe moves could be identified!' + + return list(move['moves'].nodes[1:]), None + @ocs_agent.param('starting_index', type=int, default=0) def exercise(self, session, params): """exercise(starting_index=0) @@ -1852,6 +2438,16 @@ def add_agent_args(parser_in=None): nargs='+', help="One or more axes to ignore.") pgroup.add_argument("--disable-idle-reset", action='store_true', help="Disable idle_reset, even for LAT.") + pgroup.add_argument("--min-el", type=float, + help="Override the minimum el defined in platform config.") + pgroup.add_argument("--max-el", type=float, + help="Override the maximum el defined in platform config.") + pgroup.add_argument("--avoid-sun", type=int, + help="Pass 0 or 1 to disable or enable Sun avoidance. " + "Overrides the platform default config.") + pgroup.add_argument("--fov-radius", type=float, + help="Override the default field-of-view (radius in " + "degrees) for Sun avoidance purposes.") return parser_in @@ -1860,11 +2456,16 @@ def main(args=None): args = site_config.parse_args(agent_class='ACUAgent', parser=parser, args=args) + agent, runner = ocs_agent.init_site_agent(args) _ = ACUAgent(agent, args.acu_config, args.exercise_plan, startup=not args.no_processes, ignore_axes=args.ignore_axes, - disable_idle_reset=args.disable_idle_reset) + disable_idle_reset=args.disable_idle_reset, + avoid_sun=args.avoid_sun, + fov_radius=args.fov_radius, + min_el=args.min_el, + max_el=args.max_el) runner.run(agent, auto_reconnect=True) diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py new file mode 100644 index 000000000..a65b7b3e0 --- /dev/null +++ b/socs/agents/acu/avoidance.py @@ -0,0 +1,687 @@ +# Sun Avoidance +# +# The docstring below is intended for injection into the documentation +# system. + +"""When considering Sun Safety of a boresight pointing, we consider an +exclusion zone around the Sun with a user-specified radius. This is +called the ``exclusion_radius`` or the field-of-view radius. + +The Safety of the instrument at any given moment is parametrized +by two numbers: + + ``sun_dist`` + The separation between the Sun and the boresight (az, el), in + degrees. + + ``sun_time`` + The minimum time, in seconds, which must elapse before the current + (az, el) pointing of the boresight will lie within the exclusion + radius of the Sun. + +While the ``sun_dist`` is an important indicator of whether the +instrument is currently in immediate danger, the ``sun_time`` is +helpful with looking forward and avoiding positions that will soon be +dangerous. + +The user-defined policy for Sun Safety is captured in the following +settings: + + ``exclusion_radius`` + The radius, in degres, of a disk centered on the Sun that must be + avoided by the boresight. + + ``min_sun_time`` + An (az, el) position is considered unsafe (danger zone) if the + ``sun_time`` is less than the ``min_sun_time``. (Expressed in + seconds.) + + ``response_time`` + An (az, el) position is considered vulnerable (warning zone) if + the ``sun_time`` is less than the ``response_time``. This is + intended to represent the maximum amount of time it could take an + operator to reach the instrument and secure it, were motion to + stall unexpectedly. (Expressed in seconds.) + + ``el_horizon`` + The elevation (in degrees) below which the Sun may be considered + as invisible to the instrument. + + ``el_dodging`` + This setting affects how point-to-point motions are executed, with + respect to what elevations may be used in intermediate legs of the + trajectory. When this is False, the platform is restricted to + travel only at elevations that lie between the initial and the + target elevation. When True, the platform is permitted to travel + at other elevations, all the way up to the limits of the + platform. Using True is helpful to find Sun-safe trajectories in + some circumstances. But False is helpful if excess elevation + changes are potentially disturbing to the cryogenics. This + setting only affects point-to-point motions; "escape" paths will + always consider all available elevations. + + +A "Sun-safe" position is a pointing of the boresight that currently +has a ``sun_time`` that meets or exceeds the ``min_sun_time`` +parameter. + +""" + +import datetime +import math +import time + +import ephem +import numpy as np +from pixell import enmap +from so3g.proj import coords, quat + +try: + import pylab as pl +except ModuleNotFoundError: + pass + +DEG = np.pi / 180 + +HOUR = 3600 +DAY = 86400 +NO_TIME = DAY * 2 + +#: Default policy to apply when evaluating Sun-safety and planning +#: trajectories. Note the Agent code may apply different defaults, +#: based on known platform details. +DEFAULT_POLICY = { + 'exclusion_radius': 20, + 'min_el': 0, + 'max_el': 90, + 'min_az': -45, + 'max_az': 405, + 'el_horizon': 0, + 'el_dodging': False, + 'min_sun_time': HOUR, + 'response_time': HOUR * 4, +} + + +class SunTracker: + """Provide guidance on what horizion coordinate positions and + trajectories are sun-safe. + + Args: + policy (dict): Exclusion policy parameters. See module + docstring, and DEFAULT_POLICY. The policy should also include + {min,max}\\_{el,az}, giving the limits supported by those axes. + site (EarthlySite or None): Site to use; default is the SO LAT. + If not None, pass an so3g.proj.EarthlySite or compatible. + map_res (float, deg): resolution to use for the Sun Safety Map. + sun_time_shift (float, seconds): For debugging and testing, + compute the Sun's position as though it were this manys + seconds in the future. If None or zero, this feature is + disabled. + fake_now (float, seconds): For debugging and testing, replace + the tracker's computation of the current time (time.time()) + with this value. If None, this testing feature is disabled. + compute (bool): If True, immediately compute the Sun Safety Map + by calling .reset(). + base_time (unix timestamp): Store this base_time and, if compute + is True, pass it to .reset(). + + """ + + def __init__(self, policy=None, site=None, + map_res=.5, sun_time_shift=None, fake_now=None, + compute=True, base_time=None): + # Note res is stored in radians. + self.res = map_res * DEG + if sun_time_shift is None: + sun_time_shift = 0. + self.sun_time_shift = sun_time_shift + self.fake_now = fake_now + self.base_time = base_time + + # Process and store the instrument config and safety policy. + if policy is None: + policy = {} + for k in policy.keys(): + assert k in DEFAULT_POLICY + _p = dict(DEFAULT_POLICY) + _p.update(policy) + self.policy = _p + + if site is None: + # This is close enough. + site = coords.SITES['so_lat'] + site_eph = ephem.Observer() + site_eph.lon = site.lon * DEG + site_eph.lat = site.lat * DEG + site_eph.elevation = site.elev + self._site = site_eph + + if compute: + self.reset(base_time) + + def _now(self): + if self.fake_now: + return self.fake_now + return time.time() + + def _sun(self, t): + self._site.date = \ + datetime.datetime.utcfromtimestamp(t + self.sun_time_shift) + return ephem.Sun(self._site) + + def reset(self, base_time=None): + """Compute and store the Sun Safety Map for a specific + timestamp. + + This basic computation is required prior to calling other + functions that use the Sun Safety Map. + + """ + # Set a reference time -- the map of sun times is usable from + # this reference time to at least 12 hours in the future. + if base_time is None: + base_time = self._now() + + # Identify zenith (ra, dec) at base_time. + Qz = coords.CelestialSightLine.naive_az_el( + base_time, 180. * DEG, 90. * DEG).Q + ra_z, dec_z, _ = quat.decompose_lonlat(Qz) + + # Map extends from dec -80 to +80. + shape, wcs = enmap.band_geometry( + dec_cut=80 * DEG, res=self.res, proj='car') + + # The map of sun time deltas + sun_times = enmap.zeros(shape, wcs=wcs) - 1 + sun_dist = enmap.zeros(shape, wcs=wcs) - 1 + + # Quaternion rotation for each point in the map. + dec, ra = sun_times.posmap() + map_q = quat.rotation_lonlat(ra.ravel(), dec.ravel()) + + v = self._sun(base_time) + + # Get the map of angular distance to the Sun. + qsun = quat.rotation_lonlat(v.ra, v.dec) + sun_dist[:] = (quat.decompose_iso(~qsun * map_q)[0] + .reshape(sun_dist.shape) / coords.DEG) + + # Get the map where each pixel says the time delay between + # base_time and when the time when the sky coordinate will be + # in the Sun mask. + dt = -ra[0] * DAY / (2 * np.pi) + qsun = quat.rotation_lonlat(v.ra, v.dec) + qoff = ~qsun * map_q + r = quat.decompose_iso(qoff)[0].reshape(sun_times.shape) / DEG + sun_times[r <= self.policy['exclusion_radius']] = 0. + for g in sun_times: + if (g < 0).all(): + continue + # Identify pixel on the right of the masked region. + flips = ((g == 0) * np.hstack((g[:-1] != g[1:], g[-1] != g[0]))).nonzero()[0] + dt0 = dt[flips[0]] + _dt = (dt - dt0) % DAY + g[g < 0] = _dt[g < 0] + + # Fill in remaining -1 with NO_TIME. + sun_times[sun_times < 0] = NO_TIME + + # Store the sun_times map and stuff. + self.base_time = base_time + self.sun_times = sun_times + self.sun_dist = sun_dist + self.map_q = map_q + + def _azel_pix(self, az, el, dt=0, round=True, segments=False): + """Return the pixel indices of the Sun Safety Map that are + hit by the trajectory (az, el) at time dt. + + Args: + az (array of float, deg): Azimuth. + el (array of float, deg): Elevation. + dt (array of float, s): Time offset relative to the base + time, at which to evaluate the trajectory. + round (bool): If True, round results to integer (for easy + look-up in the map). + segments (bool): If True, split up the trajectory into + segments (a list of pix_ji sections) such that they don't + cross the map boundaries at any point. + + """ + az = np.asarray(az) + el = np.asarray(el) + qt = coords.CelestialSightLine.naive_az_el( + self.base_time + dt, az * DEG, el * DEG).Q + ra, dec, _ = quat.decompose_lonlat(qt) + pix_ji = self.sun_times.sky2pix((dec, ra)) + if round: + pix_ji = pix_ji.round().astype(int) + # Handle out of bounds as follows: + # - RA indices are mod-ed into range. + # - dec indices are clamped to the map edge. + j, i = pix_ji + j[j < 0] = 0 + j[j >= self.sun_times.shape[-2]] = self.sun_times.shape[-2] - 1 + i[:] = i % self.sun_times.shape[-1] + + if segments: + jumps = ((abs(np.diff(pix_ji[0])) > self.sun_times.shape[-2] / 2) + + (abs(np.diff(pix_ji[1])) > self.sun_times.shape[-1] / 2)) + jump = jumps.nonzero()[0] + starts = np.hstack((0, jump + 1)) + stops = np.hstack((jump + 1, len(pix_ji[0]))) + return [pix_ji[:, a:b] for a, b in zip(starts, stops)] + + return pix_ji + + def check_trajectory(self, az, el, t=None, raw=False): + """For a telescope trajectory (vectors az, el, in deg), assumed to + occur at time t (defaults to now), get the minimum value of + the Sun Safety Map traversed by that trajectory. Also get the + minimum value of the Sun Distance map. + + This requires the Sun Safety Map to have been computed with a + base_time in the 24 hours before t. + + Returns a dict with entries: + + - ``'sun_time'``: Minimum Sun Safety Time on the traj. + - ``'sun_time_start'``: Sun Safety Time at first point. + - ``'sun_time_stop'``: Sun Safety Time at last point. + - ``'sun_dist_min'``: Minimum distance to Sun, in degrees. + - ``'sun_dist_mean'``: Mean distance to Sun. + - ``'sun_dist_start'``: Distance to Sun, at first point. + - ``'sun_dist_stop'``: Distance to Sun, at last point. + + """ + if t is None: + t = self._now() + j, i = self._azel_pix(az, el, dt=t - self.base_time) + sun_delta = self.sun_times[j, i] + sun_dists = self.sun_dist[j, i] + + # If sun is below horizon, rail sun_dist to 180 deg. + if self.get_sun_pos(t=t)['sun_azel'][1] < self.policy['el_horizon']: + sun_dists[:] = 180. + + if raw: + return sun_delta, sun_dists + return { + 'sun_time': sun_delta.min(), + 'sun_time_start': sun_delta[0], + 'sun_time_stop': sun_delta[-1], + 'sun_dist_start': sun_dists[0], + 'sun_dist_stop': sun_dists[-1], + 'sun_dist_min': sun_dists.min(), + 'sun_dist_mean': sun_dists.mean(), + } + + def get_sun_pos(self, az=None, el=None, t=None): + """Get info on the Sun's location at time t. If (az, el) are also + specified, returns the angular separation between that + pointing and Sun's center. + + """ + if t is None: + t = self._now() + v = self._sun(t) + qsun = quat.rotation_lonlat(v.ra, v.dec) + + qzen = coords.CelestialSightLine.naive_az_el(t, 0, np.pi / 2).Q + neg_zen_az, zen_el, _ = quat.decompose_lonlat(~qzen * qsun) + + results = { + 'sun_radec': (v.ra / DEG, v.dec / DEG), + 'sun_azel': ((-neg_zen_az / DEG) % 360., zen_el / DEG), + } + if self.sun_time_shift != 0: + results['WARNING'] = 'Fake Sun Position is in use!' + + if az is not None: + qtel = coords.CelestialSightLine.naive_az_el( + t, az * DEG, el * DEG).Q + r = quat.decompose_iso(~qtel * qsun)[0] + results['sun_dist'] = r / DEG + return results + + def show_map(self, axes=None, show=True): + """Plot the Sun Safety Map and Sun Distance Map on the provided axes + (a list).""" + if axes is None: + fig, axes = pl.subplots(2, 1) + fig.tight_layout() + else: + fig = None + + imgs = [] + for axi, ax in enumerate(axes): + if axi == 0: + # Sun safe time + x = self.sun_times / HOUR + x[x == NO_TIME] = np.nan + title = 'Sun safe time (hours)' + elif axi == 1: + # Sun distance + x = self.sun_dist + title = 'Sun distance (degrees)' + im = ax.imshow(x, origin='lower', cmap='Oranges') + ji = self._azel_pix(0, np.array([90.])) + ax.scatter(ji[1], ji[0], marker='x', color='white') + ax.set_title(title) + pl.colorbar(im, ax=ax) + imgs.append(im) + + if show: + pl.show() + + return fig, axes, imgs + + def analyze_paths(self, az0, el0, az1, el1, t=None, + plot_file=None, dodging=True): + """Design and analyze a number of different paths between (az0, el0) + and (az1, el1). Return the list, for further processing and + choice. + + """ + if t is None: + t = self._now() + + if plot_file: + assert (t == self.base_time) # Can only plot "now" results. + fig, axes, imgs = self.show_map(show=False) + last_el = None + + # Test all trajectories with intermediate el. + all_moves = [] + + base = { + 'req_start': (az0, el0), + 'req_stop': (az1, el1), + 'req_time': t, + 'travel_el': (el0 + el1) / 2, + 'travel_el_confined': True, + 'direct': True, + } + + # Suitable list of test els. + el_lims = [self.policy[_k] for _k in ['min_el', 'max_el']] + if el0 == el1: + el_nodes = [el0] + else: + el_nodes = sorted([el0, el1]) + if dodging and (el_lims[0] < el_nodes[0]): + el_nodes.insert(0, el_lims[0]) + if dodging and (el_lims[1] > el_nodes[-1]): + el_nodes.append(el_lims[1]) + + el_sep = 1. + el_cands = [] + for i in range(len(el_nodes) - 1): + n = math.ceil((el_nodes[i + 1] - el_nodes[i]) / el_sep) + assert (n >= 1) + el_cands.extend(list( + np.linspace(el_nodes[i], el_nodes[i + 1], n + 1)[:-1])) + el_cands.append(el_nodes[-1]) + + for iel in el_cands: + detail = dict(base) + detail.update({ + 'direct': False, + 'travel_el': iel, + 'travel_el_confined': (iel >= min(el0, el1)) and (iel <= max(el0, el1)), + }) + moves = MoveSequence(az0, el0, az0, iel, az1, iel, az1, el1, simplify=True) + + detail['moves'] = moves + traj_info = self.check_trajectory(*moves.get_traj(), t=t) + detail.update(traj_info) + all_moves.append(detail) + if plot_file and (last_el is None or abs(last_el - iel) > 5): + c = 'black' + for j, i in self._azel_pix(*moves.get_traj(), round=True, segments=True): + for ax in axes: + a, = ax.plot(i, j, color=c, lw=1) + last_el = iel + + # Include the direct path, but put in "worst case" details + # based on all "confined" paths computed above. + direct = dict(base) + direct['moves'] = MoveSequence(az0, el0, az1, el1) + traj_info = self.check_trajectory(*direct['moves'].get_traj(), t=t) + direct.update(traj_info) + conf = [m for m in all_moves if m['travel_el_confined']] + if len(conf): + for k in ['sun_time', 'sun_dist_min', 'sun_dist_mean']: + direct[k] = min([m[k] for m in conf]) + all_moves.append(direct) + + if plot_file: + # Add the direct traj, in blue. + segments = self._azel_pix(*direct['moves'].get_traj(), round=True, segments=True) + for ax in axes: + for j, i in segments: + ax.plot(i, j, color='blue') + for seg, rng, mrk in [(segments[0], slice(0, 1), 'o'), + (segments[-1], slice(-1, None), 'x')]: + ax.scatter(seg[1][rng], seg[0][rng], marker=mrk, color='blue') + # Add the selected trajectory in green. + selected = self.select_move(all_moves)[0] + if selected is not None: + traj = selected['moves'].get_traj() + segments = self._azel_pix(*traj, round=True, segments=True) + for ax in axes: + for j, i in segments: + ax.plot(i, j, color='green') + + pl.savefig(plot_file) + return all_moves + + def find_escape_paths(self, az0, el0, t=None, + debug=False): + """Design and analyze a number of different paths that move from (az0, + el0) to a sun safe position. Return the list, for further + processing and choice. + + """ + if t is None: + t = self._now() + + az_cands = [] + _az = math.ceil(self.policy['min_az'] / 180) * 180 + while _az <= self.policy['max_az']: + az_cands.append(_az) + _az += 180. + + # Clip el0 into the allowed range. + el0 = np.clip(el0, self.policy['min_el'], self.policy['max_el']) + + # Preference is to not change altitude; but allow for lowering. + n_els = math.ceil(el0 - self.policy['min_el']) + 1 + els = np.linspace(el0, self.policy['min_el'], n_els) + + path = None + for el1 in els: + paths = [self.analyze_paths(az0, el0, _az, el1, t=t, dodging=False) + for _az in az_cands] + best_paths = [self.select_move(p)[0] for p in paths] + best_paths = [p for p in best_paths if p is not None] + if len(best_paths): + path = self.select_move(best_paths)[0] + if debug: + cands, _ = self.select_move(best_paths, raw=True) + return cands + if path is not None: + return path + + return None + + def select_move(self, moves, raw=False): + """Given a list of possible "moves", select the best one. + The "moves" should be like the ones returned by + ``analyze_paths``. + + The best move is determined by first screening out dangerous + paths (ones that pass close to Sun, move closer to Sun + unnecessarily, violate axis limits, etc.) and then identifying + paths that minimize danger (distance to Sun; Sun time) and + path length. + + If raw=True, a debugging output is returned; see code. + + Returns: + (dict, list): (best_move, decisions) + + ``best_move`` -- the element of moves that is safest. If no + safe move was found, None is returned. + + ``decisions`` - List of dicts, in one-to-one correspondence + with ``moves``. Each decision dict has entries 'rejected' + (True or False) and 'reason' (string description of why the + move was rejected outright). + + """ + _p = self.policy + + decisions = [{'rejected': False, + 'reason': None} for m in moves] + + def reject(d, reason): + d['rejected'] = True + d['reason'] = reason + + # According to policy, reject moves outright. + for m, d in zip(moves, decisions): + if d['rejected']: + continue + + els = m['req_start'][1], m['req_stop'][1] + + if (m['sun_time_start'] < _p['min_sun_time']): + # If the path is starting in danger zone, then only + # enforce that the move takes the platform to a better place. + + # Test > res, rather than > 0... near the minimum this + # can be noisy. + if m['sun_dist_start'] - m['sun_dist_min'] > self.res / DEG: + reject(d, 'Path moves even closer to sun.') + continue + if m['sun_time_stop'] < _p['min_sun_time']: + reject(d, 'Path does not end in sun-safe location.') + continue + + elif m['sun_time'] < _p['min_sun_time']: + reject(d, 'Path too close to sun.') + continue + + if m['travel_el'] < _p['min_el']: + reject(d, 'Path goes below minimum el.') + continue + + if m['travel_el'] > _p['max_el']: + reject(d, 'Path goes above maximum el.') + continue + + if not _p['el_dodging']: + if m['travel_el'] < min(*els): + reject(d, 'Path dodges (goes below necessary el range).') + continue + if m['travel_el'] > max(*els): + reject(d, 'Path dodges (goes above necessary el range).') + + cands = [m for m, d in zip(moves, decisions) + if not d['rejected']] + if len(cands) == 0: + return None, decisions + + def metric_func(m): + # Sorting key for move proposals. More preferable paths + # should have higher sort order. + azs = m['req_start'][0], m['req_stop'][0] + els = m['req_start'][1], m['req_stop'][1] + return ( + # Low sun_time is bad, though anything longer + # than response_time is equivalent. + m['sun_time'] if m['sun_time'] < _p['response_time'] else _p['response_time'], + + # Single leg moves are preferred, for simplicity. + m['direct'], + + # Higher minimum sun distance is preferred. + m['sun_dist_min'], + + # Shorter paths (less total az / el motion) are preferred. + -(abs(m['travel_el'] - els[0]) + abs(m['travel_el'] - els[1])), + -abs(azs[1] - azs[0]), + + # Larger mean Sun distance is preferred. But this is + # subdominant to path length; otherwise spinning + # around a bunch of times can be used to lower the + # mean sun dist! + m['sun_dist_mean'], + + # Prefer higher elevations for the move, all else being equal. + m['travel_el'], + ) + cands.sort(key=metric_func) + if raw: + return [(c, metric_func(c)) for c in cands], decisions + return cands[-1], decisions + + +class MoveSequence: + def __init__(self, *args, simplify=False): + """Container for a series of (az, el) positions. Pass the + positions to the constructor as (az, el) tuples:: + + MoveSequence((60, 180), (60, 90), (50, 90)) + + or equivalently as individual arguments:: + + MoveSequence(60, 180, 60, 90, 50, 90) + + If simplify=True is passed, then any immediate position + repetitions are deleted. + + """ + self.nodes = [] + if len(args) == 0: + return + is_tuples = [isinstance(a, tuple) for a in args] + if all(is_tuples): + pass + elif any(is_tuples): + raise ValueError('Constructor accepts tuples or az, el, az, el; not a mix.') + else: + assert (len(args) % 2 == 0) + args = [(args[i], args[i + 1]) for i in range(0, len(args), 2)] + for (az, el) in args: + self.nodes.append((az, el)) + if simplify: + # Remove repeated nodes. + idx = 0 + while idx < len(self.nodes) - 1: + if self.nodes[idx] == self.nodes[idx + 1]: + self.nodes.pop(idx + 1) + else: + idx += 1 + + def get_legs(self): + """Iterate over the legs of the MoveSequence; yields each ((az_start, + el_start), (az_end, az_end)). + + """ + for i in range(len(self.nodes) - 1): + yield self.nodes[i:i + 2] + + def get_traj(self, res=0.5): + """Return (az, el) vectors with the full path for the MoveSequence. + No step in az or el will be greater than res. + + """ + xx, yy = [], [] + for (x0, y0), (x1, y1) in self.get_legs(): + n = max(2, math.ceil(abs(x1 - x0) / res), math.ceil(abs(y1 - y0) / res)) + xx.append(np.linspace(x0, x1, n)) + yy.append(np.linspace(y0, y1, n)) + return np.hstack(tuple(xx)), np.hstack(tuple(yy)) diff --git a/tests/agents/test_acu_agent.py b/tests/agents/test_acu_agent.py index 921fe550c..9afa6f280 100644 --- a/tests/agents/test_acu_agent.py +++ b/tests/agents/test_acu_agent.py @@ -1 +1,37 @@ +from socs.agents.acu import avoidance as av from socs.agents.acu.agent import ACUAgent # noqa: F401 + + +def test_avoidance(): + az0, el0 = 72, 67 + t0 = 1698850000 + + sun = av.SunTracker(fake_now=t0) + pos = sun.get_sun_pos() + az, el = pos['sun_azel'] + assert abs(az - az0) < .5 and abs(el - el0) < .5 + + # Zenith should be about 23 deg away. + assert abs(sun.get_sun_pos(0, 90)['sun_dist'] - 23) < 0.5 + + # Unsafe positions. + assert sun.check_trajectory([90], [60])['sun_time'] == 0 + + # Safe positions + assert sun.check_trajectory([90], [20])['sun_time'] > 0 + assert sun.check_trajectory([270], [60])['sun_time'] > 0 + + # No safe moves to Sun position. + paths = sun.analyze_paths(180, 20, az0, el0) + path, analysis = sun.select_move(paths) + assert path is None + + # Escape paths. + path = sun.find_escape_paths(az0, el0 - 5) + assert path is not None + path = sun.find_escape_paths(az0, el0 + 5) + assert path is not None + path = sun.find_escape_paths(az0 - 10, el0) + assert path is not None + path = sun.find_escape_paths(az0 + 10, el0) + assert path is not None