Repository: autorope/donkeycar (language: python)

Issue:
Update path_follow to support velocity target on a waypoint
The current path_follow algorithm searches for the nearest waypoint to it's current position, then chooses the next waypoint in the path and uses those two points to create a line; it then determines which side of the line it is on and how far from the line it is and uses that as the input to a PID that decides which direction to turn in and how much to turn.  Throttle is held constant at some value > 0 and <= 1.0 specified in configuration.

Because throttle is constant, in practice we set it to limit the speed to what we can handle driving around the tightest corner.  That means we are much slower on a long straight then we could be, or conversely we are too fast heading into a corner and so may overshoot.

Modify the waypoint data structure to include a target throttle to be achieved at that waypoint.  Modify the vehicle loop in the path_follow.py template to set the throttle based on the waypoint that we are headed to (so the next waypoint from the closest one, as described above).  Allow this to be overridden by a constant throttle if it is specified in the configuration; if no constant throttle is specified then use the throttle values in the waypoints (path) file.



Discussion / hints:
Nice! I've been hoping variable throttle would come and here it is! It's the only way to be competitive
@Ezward,

My 4WD Traxxas E-Maxx can easily do 12-15 m/sec on the straightaways, but would tip over on the curves at that velocity.
Presently I run the E-Maxx at around 3.2 m/sec (`PID_THROTTLE = 0.20`) to avoid tipping over on the curves.
Reducing the vehicle velocity on the curves to a sustainable level and increasing the velocity to a predefined maximum on the straightaways would make for some very interesting racing competition.

TCIII 
@zlite,

Doesn't ArduRover already have a variable throttle for straightaways and curves (turns)?

TCIII 
@Ezward I'm with a team of students at UCSD using donkeycar for a class project. We're interested in GPS path following and would like to try and implement a solution to address this issue. Just wanted to put it out there so you know we are working on it! 😄
@evanphilipsmith,

I am TCIII, a DC Maintainer, and I have a Traxxas E-Maxx Truck Chassis test vehicle running the DC `main branch` and using the `path_follow.py` template that employs input from a Sparkfun NEO-M9N GPS module with an actve 32db gain antenna.

I will be available to test and validate any code branches that add velocity, heading, and IMU input to the GPS CTE input mix.

Regards,
TCIII 
@evanphilipsmith,

[Improve](https://github.com/autorope/donkeycar/issues/1060) DC imu.py part. 

TCIII

Fix patch:
diff --git a/donkeycar/parts/path.py b/donkeycar/parts/path.py
index 5d79acff7..ef42a6884 100644
--- a/donkeycar/parts/path.py
+++ b/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
diff --git a/donkeycar/templates/cfg_complete.py b/donkeycar/templates/cfg_complete.py
index 913f0e843..fe3e956eb 100644
--- a/donkeycar/templates/cfg_complete.py
+++ b/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
 
diff --git a/donkeycar/templates/cfg_path_follow.py b/donkeycar/templates/cfg_path_follow.py
index dfe26f723..695b7f01b 100644
--- a/donkeycar/templates/cfg_path_follow.py
+++ b/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
 
diff --git a/donkeycar/templates/cfg_simulator.py b/donkeycar/templates/cfg_simulator.py
index f238364b3..ea498fc5d 100644
--- a/donkeycar/templates/cfg_simulator.py
+++ b/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
 
diff --git a/donkeycar/templates/path_follow.py b/donkeycar/templates/path_follow.py
index 3c991caaf..dea0e4429 100644
--- a/donkeycar/templates/path_follow.py
+++ b/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
diff --git a/setup.py b/setup.py
index 06e809865..4a0f64f37 100644
--- a/setup.py
+++ b/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',


Test patch:
diff --git a/donkeycar/tests/test_path.py b/donkeycar/tests/test_path.py
index 983052df7..b82dceb8e 100644
--- a/donkeycar/tests/test_path.py
+++ b/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):