Support velocity target on a waypoint in path_follow

donkeycar/parts/path.py

@@ -5,6 +5,7 @@
 import numpy
 from PIL import Image, ImageDraw
 
+from donkeycar.parts.transform import PIDController
 from donkeycar.utils import norm_deg, dist, deg2rad, arr_to_img, is_number_type
 
 
@@ -76,6 +77,50 @@ def load(self, filename):
 
         self.recording = False
 
+class CsvThrottlePath(AbstractPath):
+    def __init__(self, min_dist: float = 1.0) -> None:
+        super().__init__(min_dist)
+        self.throttles = []
+
+    def run(self, recording: bool, x: float, y: float, throttle: float) -> tuple:
+        if recording:
+            d = dist(x, y, self.x, self.y)
+            if d > self.min_dist:
+                logging.info(f"path point: ({x},{y}) throttle: {throttle}")
+                self.path.append((x, y))
+                self.throttles.append(throttle)
+                self.x = x
+                self.y = y
+        return self.path, self.throttles
+
+    def reset(self) -> bool:
+        super().reset()
+        self.throttles = []
+        return True
+
+    def save(self, filename: str) -> bool:
+        if self.length() > 0:
+            with open(filename, 'w') as outfile:
+                for (x, y), v in zip(self.path, self.throttles):
+                    outfile.write(f"{x}, {y}, {v}\n")
+            return True
+        else:
+            return False
+
+    def load(self, filename: str) -> bool:
+        path = pathlib.Path(filename)
+        if path.is_file():
+            with open(filename, "r") as infile:
+                self.path = []
+                for line in infile:
+                    xy = [float(i.strip()) for i in line.strip().split(sep=",")]
+                    self.path.append((xy[0], xy[1]))
+                    self.throttles.append(xy[2])
+            return True
+        else:
+            logging.warning(f"File '{filename}' does not exist")
+            return False
+
 
 class RosPath(AbstractPath):
     def __init__(self, min_dist=1.):
@@ -385,11 +430,25 @@ def run(self, path, x, y, from_pt=None):
 
 class PID_Pilot(object):
 
-    def __init__(self, pid, throttle):
+    def __init__(
+            self,
+            pid: PIDController,
+            throttle: float,
+            use_constant_throttle: bool = False,
+            min_throttle: float = None) -> None:
         self.pid = pid
         self.throttle = throttle
+        self.use_constant_throttle = use_constant_throttle
+        self.variable_speed_multiplier = 1.0
+        self.min_throttle = min_throttle if min_throttle is not None else throttle
 
-    def run(self, cte):
+    def run(self, cte: float, throttles: list, closest_pt_idx: int) -> tuple:
         steer = self.pid.run(cte)
-        logging.info("CTE: %f steer: %f" % (cte, steer))
-        return steer, self.throttle
+        if self.use_constant_throttle or throttles is None or closest_pt_idx is None:
+            throttle = self.throttle
+        elif throttles[closest_pt_idx] * self.variable_speed_multiplier < self.min_throttle:
+            throttle = self.min_throttle
+        else:
+            throttle = throttles[closest_pt_idx] * self.variable_speed_multiplier
+        logging.info(f"CTE: {cte} steer: {steer} throttle: {throttle}")
+        return steer, throttle

donkeycar/templates/cfg_complete.py

@@ -601,6 +601,7 @@
 PID_I = 0.000                       # integral mult for PID path follower
 PID_D = -0.2                        # differential mult for PID path follower
 PID_THROTTLE = 0.2                  # constant throttle value during path following
+USE_CONSTANT_THROTTLE = False       # whether or not to use the constant throttle or variable throttle captured during path recording
 SAVE_PATH_BTN = "cross"             # joystick button to save path
 RESET_ORIGIN_BTN = "triangle"       # joystick button to press to move car back to origin
 

donkeycar/templates/cfg_path_follow.py

@@ -647,6 +647,7 @@
 PID_I = 0.000                       # integral mult for PID path follower
 PID_D = -0.3                        # differential mult for PID path follower
 PID_THROTTLE = 0.50                 # constant throttle value during path following
+USE_CONSTANT_THROTTLE = False       # whether or not to use the constant throttle or variable throttle captured during path recording
 PID_D_DELTA = 0.25                  # amount the inc/dec function will change the D value
 PID_P_DELTA = 0.25                  # amount the inc/dec function will change the P value
 

donkeycar/templates/cfg_simulator.py

@@ -264,6 +264,7 @@
 PID_I = 0.000                       # integral mult for PID path follower
 PID_D = -0.2                        # differential mult for PID path follower
 PID_THROTTLE = 0.2                  # constant throttle value during path following
+USE_CONSTANT_THROTTLE = False       # whether or not to use the constant throttle or variable throttle captured during path recording
 SAVE_PATH_BTN = "cross"             # joystick button to save path
 RESET_ORIGIN_BTN = "triangle"       # joystick button to press to move car back to origin
 

donkeycar/templates/path_follow.py

@@ -57,7 +57,7 @@
 
 import donkeycar as dk
 from donkeycar.parts.controller import JoystickController
-from donkeycar.parts.path import CsvPath, PathPlot, CTE, PID_Pilot, \
+from donkeycar.parts.path import CsvThrottlePath, PathPlot, CTE, PID_Pilot, \
     PlotCircle, PImage, OriginOffset
 from donkeycar.parts.transform import PIDController
 from donkeycar.parts.kinematics import TwoWheelSteeringThrottle
@@ -211,8 +211,8 @@ def run(self, mode):
 
     # This is the path object. It will record a path when distance changes and it travels
     # at least cfg.PATH_MIN_DIST meters. Except when we are in follow mode, see below...
-    path = CsvPath(min_dist=cfg.PATH_MIN_DIST)
-    V.add(path, inputs=['recording', 'pos/x', 'pos/y'], outputs=['path'])
+    path = CsvThrottlePath(min_dist=cfg.PATH_MIN_DIST)
+    V.add(path, inputs=['recording', 'pos/x', 'pos/y', 'user/throttle'], outputs=['path', 'throttles'])
 
     if cfg.DONKEY_GYM:
         lpos = LoggerPart(inputs=['dist/left', 'dist/right', 'dist', 'pos/pos_x', 'pos/pos_y', 'yaw'], level="INFO", logger="simulator")
@@ -290,8 +290,8 @@ def reset_origin():
 
     # This will use the cross track error and PID constants to try to steer back towards the path.
     pid = PIDController(p=cfg.PID_P, i=cfg.PID_I, d=cfg.PID_D)
-    pilot = PID_Pilot(pid, cfg.PID_THROTTLE)
-    V.add(pilot, inputs=['cte/error'], outputs=['pilot/angle', 'pilot/throttle'], run_condition="run_pilot")
+    pilot = PID_Pilot(pid, cfg.PID_THROTTLE, cfg.USE_CONSTANT_THROTTLE, min_throttle=cfg.PID_THROTTLE)
+    V.add(pilot, inputs=['cte/error', 'throttles', 'cte/closest_pt'], outputs=['pilot/angle', 'pilot/throttle'], run_condition="run_pilot")
 
     def dec_pid_d():
         pid.Kd -= cfg.PID_D_DELTA

donkeycar/tests/test_path.py

@@ -2,7 +2,7 @@
 import tempfile
 import unittest
 
-from donkeycar.parts.path import CsvPath, CTE, OriginOffset
+from donkeycar.parts.path import CsvPath, CsvThrottlePath, CTE, OriginOffset
 
 
 class TestCsvPath(unittest.TestCase):
@@ -56,6 +56,68 @@ def test_csvpath_load(self):
             self.assertEqual((3, 4), xy[1])
 
 
+class TestCsvThrottlePath(unittest.TestCase):
+    def test_csvthrottlepath_run(self) -> None:
+        path = CsvThrottlePath()
+        xy, throttles = path.run(True, 1, 2, 1.0)
+        self.assertListEqual([(1, 2)], xy)
+        self.assertListEqual([1.0], throttles)
+        xy, throttles = path.run(True, 3, 4, 0.5)
+        self.assertListEqual([(1, 2), (3, 4)], xy)
+        self.assertListEqual([1.0, 0.5], throttles)
+        self.assertEqual(2, len(path.get_xy()))
+        self.assertEqual(2, len(path.throttles))
+
+    def test_csvthrottlepath_run_not_recording(self) -> None:
+        path = CsvThrottlePath()
+        xy, throttles = path.run(True, 1, 2, 1.0)
+        self.assertListEqual([(1, 2)], xy)
+        self.assertListEqual([1.0], throttles)
+        xy, throttles = path.run(False, 3, 4, 2.0)
+        self.assertListEqual([(1, 2)], xy)
+        self.assertListEqual([1.0], throttles)
+        xy, throttles = path.run(True, 3, 4, 0.5)
+        self.assertListEqual([(1, 2), (3, 4)], xy)
+        self.assertListEqual([1.0, 0.5], throttles)
+        self.assertEqual(2, len(path.get_xy()))
+        self.assertEqual(2, len(path.throttles))
+
+    def test_csvthrottlepath_save(self) -> None:
+        path = CsvThrottlePath()
+        path.run(True, 1, 2, 1.0)
+        path.run(True, 3, 4, 0.5)
+        with tempfile.TemporaryDirectory() as td:
+            filename = os.path.join(td, "test.csv")
+            path.save(filename)
+            with open(filename, "r") as testfile:
+                lines = testfile.readlines()
+                self.assertEqual(2, len(lines))
+                self.assertEqual("1, 2, 1.0", lines[0].strip())
+                self.assertEqual("3, 4, 0.5", lines[1].strip())
+
+    def test_csvthrottlepath_reset(self) -> None:
+        path = CsvThrottlePath()
+        path.run(True, 1, 2, 1.0)
+        path.run(True, 3, 4, 0.5)
+        path.reset()
+        self.assertEqual([], path.get_xy())
+
+    def test_csvthrottlepath_load(self) -> None:
+        path = CsvThrottlePath()
+        path.run(True, 1, 2, 1.0)
+        path.run(True, 3, 4, 0.5)
+        with tempfile.TemporaryDirectory() as td:
+            filename = os.path.join(td, "test.csv")
+            path.save(filename)
+            path.reset()
+            path.load(filename)
+            xy = path.get_xy()
+            self.assertEqual(2, len(xy))
+            self.assertEqual((1, 2), xy[0])
+            self.assertEqual((3, 4), xy[1])
+            self.assertListEqual([1.0, 0.5], path.throttles)
+
+
 class TestCTE(unittest.TestCase):
 
     def test_nearest_two_pts(self):

setup.py

@@ -24,7 +24,7 @@ def package_files(directory, strip_leading):
     long_description = fh.read()
 
 setup(name='donkeycar',
-      version="4.4.3-main",
+      version="4.4.4-main",
       long_description=long_description,
       description='Self driving library for python.',
       url='https://github.com/autorope/donkeycar',