github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 10:38:27 -05:00
Code Issues Packages Projects Releases Wiki Activity

release grid based_sweep_coverage_path_planner.py

Browse Source
This commit is contained in:
Atsushi Sakai
2019-07-11 21:12:46 +09:00
parent 6201aa262e
commit 06dfc05d89
2 changed files with 21 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -5,9 +5,11 @@ Path tracking simulation with pure pursuit steering control and PID speed contro
author: Atsushi Sakai
"""
import numpy as np
import math
import matplotlib.pyplot as plt
import numpy as np
import unicycle_model
Kp = 2.0 # speed propotional gain
@@ -75,7 +77,7 @@ def calc_target_index(state, cx, cy):
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
dy = cy[ind + 1] - cy[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
@@ -153,6 +155,7 @@ def set_stop_point(target_speed, cx, cy, cyaw):
forward = True
d = []
is_back = False
# Set stop point
for i in range(len(cx) - 1):