Support velocity target on a waypoint in path_follow

[e13h]

Added support for variable throttle in path_follow by modifying the waypoint data structure to record the throttle at each waypoint. The feature may be turned off by setting USE_CONSTANT_THROTTLE = True in your myconfig.py (the flag is set to False by default).

Closes #1047.

[Ezward]

Evan, thanks for doing this, this is very valuable work. I still have to go through the PR in more depth, but I have a higher level comment that you could tackle pretty easily. The code implements a variable throttle; it records the throttle value at each waypoint. The running code uses the throttle of the closest waypoint. That is all very good. The issue is the naming; in the code it is referred to as velocity, but it is actually throttle. I think we might want to implement true velocity control in a future update (where we try to match a velocity in meters/second), so I'd rather not use that term here.

[e13h]

Great point! I just updated the naming conventions in the code.

[Ezward]

@evanphilipsmith the code looks good; just a small amount of code to add a nice capability; very good. My question now; has this been tested; do you have a video of this working; it would be nice once we get this merged to have a video to show to the community when we announce the feature.

[Ezward]

@TCIII would you be able to give this a test?

[Ezward]

@evanphilipsmith can you add unit tests. You can clone most of them from https://github.com/evanphilipsmith/donkeycar/blob/8a6cbd6da2e2c8a56ebccedcb0b1323c9908aae5/donkeycar/tests/test_path.py#L8

[TCIII]

@Ezward,

Yes. I assume that you will create a branch for testing purposes?

TCIII

[DocGarbanzo]

Looks good to me in general, but the pytorch tests are failing, it doesn't look like it's related to your change. Will have to take a look.

[Ezward]

@Ezward,

Yes. I assume that you will create a branch for testing purposes?

TCIII

We use Evan's branch on his repo. So clone his repo and checkout that branch and do an install of donkey.

[TCIII]

@evanphilipsmith,

I believe that I have identified an issue with the "Support velocity target on a waypoint in path_follow" effort:

I have uncovered a conundrum: To get accurate GPS coordinates during path recording the user needs to go slow especially when the data rate for the GPS is set at 5 Hz like mine is.
So the path_follow template now records the path recording "velocity" throttle position and not the actual velocity that the vehicle can travel the course at.
Constant throttle management during path playback, which is what is being used presently, is set by PID_THROTTLE = 0.XX # constant throttle value during path following.
So if the user goes fast on the straightaways, instead of slow, during path recording there is a possible chance of missing a GPS coordinate data point. Going slow around curves or turns is not as bad as the user has to slowdown in those instances anyway.

On the other hand I have gone fast on the straightaways during path recording and it did not seem to make a whole lot of difference in the path tracking performance during path playback.

TCIII

[Ezward]

I don't think this is an issue with the code; it is an issue for slowly updating GPS receivers. It inevitable that GPS receivers that have slow updates will end up with larger distances between waypoints the faster the car travels. As you say, the driver will slow down for turns so the turn should have more waypoints per unit distance. Theoretically a straight needs only two points to define it.

I think we are not trying to make a slowly updating GPS perform like a better GPS in this PR. The driver needs to stay within what the GPS receiver and CTE algorithm can handle.

However, you do bring up an issue that could be an problem, but I think it is actually a bigger issue for curves than straights. The CTE algorithm 'draws' lines between waypoints; that can cause oversteering because it cuts the curve; we might want to look at fitting curves to the waypoints and interpolate more waypoints to avoid this issue. That would come in another PR if we feel we want/need it.

[Ezward]

@evanphilipsmith During real world testing @TCIII found that we may end up recording a target velocity zero or near zero, especially at the start or end of recording the path. This is particularly bad at the start as it causes the vehicle to stall at the start and make no progress. I can see a possible solution; create a configuration for minimum autopilot throttle and make sure we enforce this during autopilot. @TCIII will add a comment that includes the path he recording that shows the issue.

[TCIII]

@evanphilipsmith,

There was a throttle issue with evan's path_follow template code.
You can see from the attached donkey_path.csv file that the instantaneous throttle value was being recorded, however when playing back the recorded path the E-Maxx did not start moving from the origin when USE_CONSTANT_THROTTLE = False and just sat there. I could see in the CLI window that the throttle was constantly -0.01 during playback even though the x and y coordinates were changing as they do when USE_CONSTANT_THROTTLE = True.
It was like the throttle value did not update as the x and y coordinates updated and stayed frozen at -0.01, which you can see from the donkey_path.csv file, was the starting throttle and final throttle value at the beginning and ending of the course.

Regards,
TCIII
donkey_path.csv

[TCIII]

@evanphilipsmith,

Since donkey_path.csv can be edited with either Excel or nano, I recorded a new path this morning and made the -0.01 starting throttle 0.2 and I changed the throttle settings on the approach to the origin to 0.2 also and began testing the updated donkey_path.csv file with the E-Maxx.
My test course for my E-Maxx is on a residential street that runs North - South and is about 6 m wide with a 180 deg turn at each end.
Your code works great. The E-Maxx lunged off the line and headed very quickly to the first left hand turn curve where it slowed down as it went around the North end curve then accelerated to high speed on the South bound straightaway until it slowed down for the next left hand turn as it went around the South end curve and accelerated back to the origin on the North bound straightaway to start the course over again.

It would be nice to be able to superimpose the GPS coordinates on a sky view map of the recorded course and be able to edit the throttle values based on where the car is on the course at any given instant.
Being able to edit the throttle values based on where the car is on the course at any given time would allow a user to record the course at a slow speed for accuracy and then up the throttle values to match the curves and straightaways.
I can tell by the throttle values when the car is on the straightaways and bump up the recorded throttle values to higher values. I could also improve the throttle values in the turns.

Nice work Evan. This is going to give Chrisa heartburn Ezward.

TCIII

[DocGarbanzo]

@evanphilipsmith, we would like to merge your PR, could you please address the couple of small comments, mainly the unit test and I had a few small suggestions on the code. It would be great, if you could also bump the version (patch+=1). I have also re-based the branch, because we sorted the failing pytorch tests in the meantime.

[e13h]

Thank you all for your review. Sorry for the radio silence on my part this last week, I was out of town. I will gladly implement the changes requested over the next few days and request another review.

[Ezward]

@evanphilipsmith Hi, I hope you had a good break. Have you been able to get back to this? #1068 (comment)

[e13h]

Hi, thanks for being patient with me. After a nice long holiday break I have finally gotten around to implementing the changes requested. Here's a brief summary of my recent changes:

• Unit tests
• Record throttles using a minimum threshold
  • Using the PID_THROTTLE config variable, which is the same value that the autopilot drives with when USE_CONSTANT_THROTTLE=True (let me know if you think this deserves a separate config variable instead)
• Bump version patch number
• Formatting issues and minor tweaks

I have re-requested your review and look forward to your comments.

[Ezward]

@evanphilipsmith will you be at the Feb 25th race at UCSD? I will not be able to make it; I hope you have a chance to use this feature you have created.

[e13h]

@Ezward I am interested in attending the race, but I'll have to see if there is a spare RC car I can borrow from the lab since last fall I was borrowing the hardware as part of the course.

I updated the code to enforce a minimum throttle during autopilot instead of during recording. Let me know if you see any more issues.

[Ezward]

@evanphilipsmith thanks, @TCIII will give this change a test.

[TCIII]

@Ezward I am interested in attending the race, but I'll have to see if there is a spare RC car I can borrow from the lab since last fall I was borrowing the hardware as part of the course.

I updated the code to enforce a minimum throttle during autopilot instead of during recording. Let me know if you see any more issues.

@evanphilipsmith,
That is a good change as I set my PID_THROTTLE value at 0.2 which is perfect for a minimum throttle value during autopilot playback of the recorded path at the start from the origin where the test vehicle is not moving initially.
I suspect that my straightaway recorded throttle values are above 0.5 where as the recorded throttle values for twists and turns are between 0.2 - 0.3.

I just downloaded and installed your path-follow-velocity-target branch on my Rpi 3B+/Robo HAT MM1 configured Traxxas E-Maxx today and will test this weekend.

TCIII

[Ezward]

There is a runtime error; see comment on line 214 of path_follow.py

[Ezward]

@TCIII edited his manage.py line 214 to remove the runtime error and tested the branch outside. It worked really well. So once we get that fixed, then I can approve and we can merge.

[e13h]

Ok, I pushed up the change. Does it look right now?

[Ezward]

Even, thanks for creating this great feature. Tom has been testing and if very excited by the results. I will merge the PR and then update the docs.

[e13h]

@DocGarbanzo I see that one of the required checks failed. I'm not sure if it is related to my changes or, if like last time the checks failed, it is unrelated. Do you have any tips for how to fix the issue?

[DocGarbanzo]

@evanphilipsmith - this has nothing to do w/ your PR. We are also seeing this with other commits. It seems to be cause by some dpendency conflicts between mamba and other packages on ubuntu only. It seems to work fine in OSX. I'll have to have a separate look at this. Will merge your PR.