Spider milovice

osgar/drivers/oak_camera_v3.py

@@ -8,7 +8,6 @@
 
 import depthai as dai
 import numpy as np
-import cv2
 
 
 g_logger = logging.getLogger(__name__)

osgar/followpath.py

@@ -6,6 +6,7 @@
 from osgar.lib.route import Route as GPSRoute, DummyConvertor
 from osgar.lib.line import Line
 from osgar.lib.mathex import normalizeAnglePIPI
+from osgar.lib import quaternion
 from osgar.node import Node
 from osgar.bus import BusShutdownException
 from osgar.exceptions import EmergencyStopException
@@ -71,14 +72,23 @@ def control(self, pose):
                 return 0, 0  # maybe different steering angle?
         return self.max_speed, normalizeAnglePIPI(angle - pose[2])
 
-    def on_pose2d(self, data):
-        x, y, heading = data
-        self.last_position = [x / 1000.0, y / 1000.0, math.radians(heading / 100.0)]
+    def drive_robot(self):
         speed, angular_speed = self.control(self.last_position)
         if self.verbose:
             print(speed, angular_speed)
         self.send_speed_cmd(speed, angular_speed)
 
+    def on_pose2d(self, data):
+        x, y, heading = data
+        self.last_position = [x / 1000.0, y / 1000.0, math.radians(heading / 100.0)]
+        self.drive_robot()
+
+    def on_pose3d(self, data):
+        [x, y, z], quat = data
+        heading = quaternion.heading(quat)
+        self.last_position = [x, y, heading]
+        self.drive_robot()
+
     def on_emergency_stop(self, data):
         if self.raise_exception_on_stop and data:
             raise EmergencyStopException()

osgar/go.py

@@ -16,6 +16,8 @@ def __init__(self, config, bus):
         self.speed = config['max_speed']
         self.dist = config['dist']
         self.timeout = timedelta(seconds=config['timeout'])
+        # stop_timeout: it keeps the application running even after a stop command has been sent.
+        self.stop_timeout = timedelta(seconds=config.get('stop_timeout', 1))
 
         self.desired_steering_angle = None  # not defined, do not publish by default
         self.desired_angular_speed = None
@@ -74,7 +76,7 @@ def sub_run(self):
                 break
         print(self.time, "STOP")
         self.send_speed_cmd(0.0, angular_speed=0.0, steering_angle=self.desired_steering_angle)
-        self.wait(timedelta(seconds=1))
+        self.wait(self.stop_timeout)
         print(self.time, "distance:", self.traveled_dist, "time:", (self.time - start_time).total_seconds())
 
     def run(self):

osgar/platforms/spider.py

@@ -16,7 +16,7 @@
 MAX_WHEEL_ANGLE = math.radians(80)
 
 
-def get_desired_angle(speed, angular_speed):
+def get_desired_angle(speed, angular_speed):  # TODO to handle the calculation
     if speed == 0:
         return 0
     # The formula is similar to Kloubak but not the same because the steering geometry is different.
@@ -229,7 +229,7 @@ def on_can(self, data):
         if status is not None:
             self.publish('status', status)
 
-    def on_move(self, data):
+    def on_desired_steering(self, data):
         speed_mm, desired_angle_cdeg = data
         self.desired_speed = speed_mm / 1000.0
         self.desired_angle = math.radians(desired_angle_cdeg / 100.0)  # one hundredth of rad
@@ -241,6 +241,11 @@ def on_desired_speed(self, data):
         angular_speed = math.radians(angular_speed_crad / 100.0)  # one hundredth of rad
         self.desired_angle = get_desired_angle(self.desired_speed, angular_speed)
 
+    def on_reset(self, data):
+        # Return some parameters to default settings. It allows multiple starts.
+        self.pose2d = (0.0, 0.0, 0.0)
+        self.already_moved = False
+
     def send_speed(self, data):
         # set PI controller
         speed_p = 200