Add simulation for local dev

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

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

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,

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?

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

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)
 

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

tasks.py

@@ -0,0 +1,5 @@
+from invoke import task
+
+@task
+def dev(ctx):
+    ctx.run('docker-compose -f docker-compose-dev.yml up')