From fc913041d241bcb4753f5a537a474120b2c0c75f Mon Sep 17 00:00:00 2001 From: Adrien Emery Date: Wed, 28 Nov 2018 22:57:50 -0800 Subject: [PATCH] Add simulation for local dev --- auv_control_pi/components/ahrs.py | 26 ++++++++++++++---- auv_control_pi/components/auv_control.py | 24 +++++++++++++---- auv_control_pi/components/gps.py | 19 ++++++++++++- auv_control_pi/components/navigation.py | 13 ++++----- auv_control_pi/utils.py | 12 ++++++++- auv_control_pi/wamp.py | 34 ++++++++++++++---------- requirements-dev.txt | 2 +- tasks.py | 5 ++++ 8 files changed, 100 insertions(+), 35 deletions(-) create mode 100644 tasks.py diff --git a/auv_control_pi/components/ahrs.py b/auv_control_pi/components/ahrs.py index 1e460cd..434377b 100644 --- a/auv_control_pi/components/ahrs.py +++ b/auv_control_pi/components/ahrs.py @@ -22,7 +22,7 @@ import os import asyncio import logging -from ..wamp import ApplicationSession, rpc +from ..wamp import ApplicationSession, rpc, subscribe PI = os.getenv('PI', False) @@ -60,6 +60,7 @@ def __init__(self, *args, **kwargs): gyro_meas_error = radians(270) # Original code indicates this leads to a 2 sec response time self.beta = sqrt(3.0 / 4.0) * gyro_meas_error # compute beta (see README) self.update_frequency = 10 + self._simulated_heading = 0 def onConnect(self): logger.info('Connecting to {} as {}'.format(self.config.realm, self.name)) @@ -85,25 +86,40 @@ def get_heading(self): def get_declination(self): return config.declination + @rpc('ahrs.get_board_offset') + def get_board_offset(self): + return config.board_offset + @rpc('ahrs.set_declination') def set_declination(self, val): self.declination = float(val) config.declination = self.declination config.save() + @rpc('ahrs.set_board_offset') + def set_board_offset(self, val): + self.board_offset = float(val) + config.board_offset = self.board_offset + config.save() + + @subscribe('auv.update') + def _simulate_heading(self, data): + if SIMULATION: + speed = data['turn_speed'] / 10 + self._simulated_heading += speed + @property def heading(self): if SIMULATION: - return 0 - + heading = self.declination + self.board_offset + self._simulated_heading + return clamp_angle(heading) else: offset = self.declination + self.board_offset heading = ( 180 + degrees(radians(offset) + atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]), self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3])) ) - heading = clamp_angle(heading) - return heading + return clamp_angle(heading) @property def pitch(self): diff --git a/auv_control_pi/components/auv_control.py b/auv_control_pi/components/auv_control.py index 529d752..23de55d 100644 --- a/auv_control_pi/components/auv_control.py +++ b/auv_control_pi/components/auv_control.py @@ -9,6 +9,13 @@ config = Configuration.get_solo() +def get_motor_speed(throttle, turn_speed): + turn_speed = abs(turn_speed) + motor_speed = 100 - turn_speed * 2 + motor_speed = round(throttle * motor_speed / 100) + return motor_speed + + class AUV(ApplicationSession): """Main entry point for controling the Mothership and AUV """ @@ -44,8 +51,7 @@ async def onJoin(self, details): # as well as doing it from web interface self.left_motor.initialize() self.right_motor.initialize() - - super().onJoin(details) + await super().onJoin(details) @rpc('auv.set_left_motor_speed') def set_left_motor_speed(self, speed): @@ -78,11 +84,11 @@ def _move(self): # left turn if turn_speed < 0: self.right_motor.speed = self.throttle - self.left_motor.speed = round(self.throttle * ((100 - abs(turn_speed)) / 100)) + self.left_motor.speed = get_motor_speed(self.throttle, turn_speed) # right turn elif turn_speed > 0: - self.right_motor.speed = round(self.throttle * ((100 - abs(turn_speed)) / 100)) + self.right_motor.speed = get_motor_speed(self.throttle, turn_speed) self.left_motor.speed = self.throttle # straight @@ -118,7 +124,7 @@ def move_center(self): @rpc('auv.set_turn_val') def set_turn_val(self, turn_speed): - self.turn_speed = turn_speed + self.turn_speed = int(turn_speed) self._move() @rpc('auv.rotate_right') @@ -141,6 +147,14 @@ def rotate_left(self, speed): self.left_motor.reverse(speed) self.right_motor.forward(speed) + @rpc('auv.set_throttle') + def set_throttle(self, throttle): + throttle = int(throttle) + throttle = max(-self.throttle_limit, throttle) + throttle = min(self.throttle_limit, throttle) + self.throttle = throttle + self._move() + @rpc('auv.forward_throttle') def forward_throttle(self, throttle=0): logger.debug('Setting forward throttle to {}'.format(throttle)) diff --git a/auv_control_pi/components/gps.py b/auv_control_pi/components/gps.py index f389a62..bb5f3a8 100644 --- a/auv_control_pi/components/gps.py +++ b/auv_control_pi/components/gps.py @@ -2,9 +2,10 @@ import asyncio import logging +from auv_control_pi.utils import point_at_distance, Point from navio.gps import GPS from ..models import GPSLog -from ..wamp import ApplicationSession, rpc +from ..wamp import ApplicationSession, rpc, subscribe logger = logging.getLogger(__name__) PI = os.getenv('PI', False) @@ -33,10 +34,20 @@ def __init__(self, *args, **kwargs): self.height_sea = None self.horizontal_accruacy = None self.vertiacl_accruracy = None + self.throttle = 0 + self.heading = 0 def onConnect(self): self.join(realm=self.config.realm) + @subscribe('auv.update') + def _update_auv(self, data): + self.throttle = data['throttle'] + + @subscribe('ahrs.update') + def _update_ahrs(self, data): + self.heading = data['heading'] + @rpc('gps.get_position') def get_position(self): return self.lat, self.lng @@ -62,6 +73,12 @@ async def update(self): if PI: msg = self.gps.update() self._parse_msg(msg) + elif SIMULATION: + if self.throttle > 0: + distance = self.throttle / 10 + new_point = point_at_distance(distance, self.heading, Point(self.lat, self.lng)) + self.lat = new_point.lat + self.lng = new_point.lng payload = { 'lat': self.lat, diff --git a/auv_control_pi/components/navigation.py b/auv_control_pi/components/navigation.py index 685fe98..1b56d7e 100644 --- a/auv_control_pi/components/navigation.py +++ b/auv_control_pi/components/navigation.py @@ -69,18 +69,14 @@ def get_target_waypoint_distance(self): 'target_waypoint_distance': config.target_waypoint_distance } - @rpc('nav.get_target_waypoint_distance') + @rpc('nav.set_target_waypoint_distance') def set_target_waypoint_distance(self, target_waypoint_distance): - config.target_waypoint_distance = target_waypoint_distance - config.save() - - @rpc('nav.set_navigation_values') - def set_navigation_values(self, target_waypoint_distance): - config.target_waypoint_distance = target_waypoint_distance + config.target_waypoint_distance = int(target_waypoint_distance) config.save() @rpc('nav.move_to_waypoint') def move_to_waypoint(self, waypoint): + self.call('auv.forward_throttle', 50) self.pid.auto_mode = True if isinstance(waypoint, dict): waypoint = Point(**waypoint) @@ -124,6 +120,7 @@ async def update(self): except IndexError: # otherwise we have arrived self.arrived = True + self.stop() logger.info('Arrived at {}'.format(self.target_waypoint)) # otherwise keep steering towards the target waypoint @@ -145,7 +142,7 @@ async def update(self): }) await asyncio.sleep(1 / self.update_frequency) - def _asteer(self): + def _steer(self): """Calculate heading error to feed into PID """ # TODO think about how often should we update the target heading? diff --git a/auv_control_pi/utils.py b/auv_control_pi/utils.py index d2978f3..f0203b6 100644 --- a/auv_control_pi/utils.py +++ b/auv_control_pi/utils.py @@ -1,6 +1,6 @@ import time from collections import deque, namedtuple -from pygc import great_distance +from pygc import great_distance, great_circle Point = namedtuple('Point', ['lat', 'lng']) @@ -21,6 +21,16 @@ def clamp_angle(deg): return deg +def point_at_distance(distance, heading, current_point): + result = great_circle( + distance=distance, + azimuth=heading, + latitude=current_point.lat, + longitude=current_point.lng + ) + return Point(result['latitude'], result['longitude']) + + def heading_to_point(point_a, point_b): """Calculate heading between two points """ diff --git a/auv_control_pi/wamp.py b/auv_control_pi/wamp.py index f7abbb7..d26353b 100644 --- a/auv_control_pi/wamp.py +++ b/auv_control_pi/wamp.py @@ -1,5 +1,6 @@ import asyncio import logging +from functools import wraps from autobahn.asyncio import ApplicationSession as AutobahnApplicationSession @@ -11,8 +12,9 @@ def subscribe(topic): raise ValueError('Must provide `topic `to subscribe to') def outer(fnc): - def inner(*args, **kwargs): - fnc(*args, **kwargs) + @wraps(fnc) + def inner(self, *args, **kwargs): + return fnc(self, *args, **kwargs) inner.is_subcription = True inner.topic = topic return inner @@ -24,8 +26,9 @@ def rpc(rpc_uri=None): raise ValueError('Must provide rpc_uri') def outer(fnc): + @wraps(fnc) def inner(*args, **kwargs): - fnc(*args, **kwargs) + return fnc(*args, **kwargs) inner.is_rpc = True inner.rpc_uri = rpc_uri return inner @@ -36,31 +39,34 @@ class ApplicationSession(AutobahnApplicationSession): name = '' @classmethod - def rpc_methods(cls): - attrs = dir(cls) + def rpc_methods(cls, instance=None): + _cls = instance or cls + attrs = dir(_cls) attrs.remove('rpc_methods') - methods = [getattr(cls, attr) for attr in attrs if callable(getattr(cls, attr))] + methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))] rpc_methods = [method for method in methods if getattr(method, 'is_rpc', False)] return {method.rpc_uri: method for method in rpc_methods} @classmethod - def rpc_uris(cls): - return cls.rpc_methods().keys() + def rpc_uris(cls, instance=None): + return cls.rpc_methods(instance).keys() - def subcribtion_handles(cls): - attrs = dir(cls) - attrs.remove('subcribtion_handles') - methods = [getattr(cls, attr) for attr in attrs if callable(getattr(cls, attr))] + @classmethod + def subcribtion_handlers(cls, instance=None): + _cls = instance or cls + attrs = dir(_cls) + attrs.remove('subcribtion_handlers') + methods = [getattr(_cls, attr) for attr in attrs if callable(getattr(_cls, attr))] handlers = [method for method in methods if getattr(method, 'is_subcription', False)] return {method.topic: method for method in handlers} async def onJoin(self, details): logger.info('Joined realm as {}'.format(self.name)) - for rpc_uri, method in self.rpc_methods().items(): + for rpc_uri, method in self.rpc_methods(self).items(): logger.debug('Registering RPC: {}'.format(rpc_uri)) await self.register(method, rpc_uri) - for topic, handler in self.subcribtion_handles().items(): + for topic, handler in self.subcribtion_handlers(self).items(): logger.debug('Subscribing To Topic: {}'.format(topic)) await self.subscribe(handler, topic) diff --git a/requirements-dev.txt b/requirements-dev.txt index d209ac4..71f37d6 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -1,5 +1,5 @@ autobahn==18.7.1 -Django>=2.1 +Django>=2.1.2 django-solo==1.1.3 envitro==0.4.2 simple-pid==0.1.4 diff --git a/tasks.py b/tasks.py new file mode 100644 index 0000000..119944c --- /dev/null +++ b/tasks.py @@ -0,0 +1,5 @@ +from invoke import task + +@task +def dev(ctx): + ctx.run('docker-compose -f docker-compose-dev.yml up')