def controller_Stanley():
"""
Steering control is similar to that used by Stanford's Stanley (DARPA Grand Challenge winner)
"""
global path, newPath, Ballbot_steering, Ballbot_speed, pub_velcmd
k = 2.0
Ballbot_speed = 0.0
Ballbot_steering = 0.0
currentindex_inPath = 0 # path index that the car is closest to
targetindex_inPath = 0 # path index that is 50 cm ahead of the car
r = rospy.Rate(10)
average_error_position = 0
ctr_error_position = 0
while not rospy.is_shutdown():
if newPath == False:
continue
while (currentindex_inPath <
len(path) - 1) and not (rospy.is_shutdown()):
Ballbot_speed = 1.0 # set speed
if (newPath == True):
# if there is a new path, restart driving along this path
newPath = False
currentindex_inPath = 0
targetindex_inPath = 0
average_error_position = 0
ctr_error_position = 0
continue
else:
currentindex_inPath = nearestNeighbor_inPath(
(Ballbot_X, Ballbot_Y, Ballbot_TH), currentindex_inPath)
targetindex_inPath = min(
len(path) - 1,
currentindex_inPath + int(targetlookahead / 5.0))
path_element = path[currentindex_inPath]
# calculate cross-track error, x_t
x_t = util.distance_Euclidean(Ballbot_X, Ballbot_Y,
path_element.pose.x,
path_element.pose.y)
average_error_position += x_t
ctr_error_position += 1
heading = (
math.atan2(path[targetindex_inPath].pose.y - Ballbot_Y,
path[targetindex_inPath].pose.x - Ballbot_X) %
(2 * math.pi))
error = Ballbot_TH - heading
"""
correct roll-over problems with error:
if abs(error) is greater than 180, then we'd rather turn the other way!
"""
if abs(error) > math.pi:
error = (2 * math.pi - abs(error)) * (-1 * cmp(error, 0))
if error < 0:
x_t = -1 * x_t
# calculate heading error, psi_t
psi_t = Ballbot_TH - path_element.pose.theta
"""
correct roll-over problems with error:
if abs(error) is greater than 180, then we'd rather turn the other way!
"""
if abs(psi_t) > math.pi:
psi_t = (2 * math.pi - abs(psi_t)) * (-1 * cmp(psi_t, 0))
# Set steering
Ballbot_steering = psi_t + math.atan(k * x_t / Ballbot_speed)
if (Ballbot_steering > math.radians(30)):
Ballbot_steering = math.radians(30)
elif (Ballbot_steering < -math.radians(30)):
Ballbot_steering = -math.radians(30)
# Speed control
cur_dir = path_element.direction
lookahead_dir = path[targetindex_inPath].direction
cur_type = path_element.type
lookahead_type = path[targetindex_inPath].type
near_obstacle = path_element.near_obstacle
Ballbot_speed = 1.0 # 2.0
if (cur_type == 't') or (lookahead_type == 't'):
Ballbot_speed = 1.0
if (near_obstacle == "t"): # if near obstacle, slow down
Ballbot_speed = 1.0
if (cur_dir !=
lookahead_dir): # if a reverse is coming up, slow down
Ballbot_speed = 0.5
if (currentindex_inPath >=
(len(path) - 20)): # if within 1 m of goal, slow down
Ballbot_speed = 1.0
if (cur_dir == 'b'): # flip sign to reverse
Ballbot_speed = -1 * abs(Ballbot_speed)
elif (cur_dir == 'f'):
Ballbot_speed = abs(Ballbot_speed)
else:
Ballbot_speed = 0.0
#Ballbot_speed = 0.0; # UNCOMMENT TO SET DRIVE SPEED to 0
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
r.sleep()
# goal reached!
rospy.loginfo("goalreached")
pub_status.publish("goalreached")
Ballbot_speed = 0
Ballbot_steering = 0
currentindex_inPath = 0
targetindex_inPath = 0
pub_velcmd.publish(Ballbot_speed, Ballbot_steering)
pub_status = rospy.Publisher('status', String)
def nearestNeighbor_inPath((x, y, th), currentindex_inPath):
"""
Given x,y,th, find the point closest to the robot in the path
"""
global path
d_min = float('inf')
index_min = 0
index = currentindex_inPath
for path_element in path[currentindex_inPath:currentindex_inPath + 10]:
"""
Search only from the currentindex to currentindex+10
"""
d = util.distance_Euclidean(path_element.pose.x, path_element.pose.y,
x, y)
if (d < d_min):
d_min = d
index_min = index
elif (d < 0.05
): # if error is less than 5 cm, take this as the nearest point
d_min = d
index_min = index
break
index += 1
return index_min
def controller_PD():
"""
Implements controller
def controller_Stanley():
"""
Steering control is similar to that used by Stanford's Stanley (DARPA Grand Challenge winner)
"""
global path,newPath,Ballbot_steering,Ballbot_speed,pub_velcmd
k = 2.0
Ballbot_speed = 0.0
Ballbot_steering = 0.0
currentindex_inPath = 0 # path index that the car is closest to
targetindex_inPath = 0 # path index that is 50 cm ahead of the car
r = rospy.Rate(10)
average_error_position = 0
ctr_error_position = 0
while not rospy.is_shutdown():
if newPath == False:
continue
while(currentindex_inPath < len(path)-1) and not (rospy.is_shutdown()):
Ballbot_speed = 1.0 # set speed
if(newPath == True):
# if there is a new path, restart driving along this path
newPath = False
currentindex_inPath = 0
targetindex_inPath = 0
average_error_position = 0
ctr_error_position = 0
continue
else:
currentindex_inPath = nearestNeighbor_inPath((Ballbot_X,Ballbot_Y,Ballbot_TH),currentindex_inPath)
targetindex_inPath = min(len(path)-1,currentindex_inPath + int(targetlookahead/5.0))
path_element = path[currentindex_inPath]
# calculate cross-track error, x_t
x_t = util.distance_Euclidean(Ballbot_X,Ballbot_Y,path_element.pose.x,path_element.pose.y)
average_error_position += x_t
ctr_error_position +=1
heading = (math.atan2(path[targetindex_inPath].pose.y-Ballbot_Y, path[targetindex_inPath].pose.x-Ballbot_X)%(2*math.pi))
error = Ballbot_TH - heading
"""
correct roll-over problems with error:
if abs(error) is greater than 180, then we'd rather turn the other way!
"""
if abs(error) > math.pi:
error = (2*math.pi - abs(error))*(-1*cmp(error,0))
if error < 0:
x_t = -1*x_t
# calculate heading error, psi_t
psi_t = Ballbot_TH - path_element.pose.theta
"""
correct roll-over problems with error:
if abs(error) is greater than 180, then we'd rather turn the other way!
"""
if abs(psi_t) > math.pi:
psi_t = (2*math.pi - abs(psi_t))*(-1*cmp(psi_t,0))
# Set steering
Ballbot_steering = psi_t + math.atan(k*x_t/Ballbot_speed)
if(Ballbot_steering > math.radians(30)):
Ballbot_steering = math.radians(30)
elif(Ballbot_steering < -math.radians(30)):
Ballbot_steering = -math.radians(30)
# Speed control
cur_dir = path_element.direction
lookahead_dir = path[targetindex_inPath].direction
cur_type = path_element.type
lookahead_type = path[targetindex_inPath].type
near_obstacle = path_element.near_obstacle
Ballbot_speed = 1.0 # 2.0
if(cur_type=='t') or (lookahead_type=='t'):
Ballbot_speed = 1.0
if(near_obstacle == "t"): # if near obstacle, slow down
Ballbot_speed = 1.0
if(cur_dir != lookahead_dir): # if a reverse is coming up, slow down
Ballbot_speed = 0.5
if(currentindex_inPath >= (len(path) - 20)): # if within 1 m of goal, slow down
Ballbot_speed = 1.0
if(cur_dir == 'b'): # flip sign to reverse
Ballbot_speed = -1*abs(Ballbot_speed)
elif(cur_dir == 'f'):
Ballbot_speed = abs(Ballbot_speed)
else:
Ballbot_speed = 0.0
#Ballbot_speed = 0.0; # UNCOMMENT TO SET DRIVE SPEED to 0
pub_velcmd.publish(Ballbot_speed,Ballbot_steering)
r.sleep()
# goal reached!
rospy.loginfo("goalreached")
pub_status.publish("goalreached")
Ballbot_speed = 0
Ballbot_steering = 0
currentindex_inPath = 0
targetindex_inPath = 0
pub_velcmd.publish(Ballbot_speed,Ballbot_steering)
pub_velcmd = rospy.Publisher('vel_cmd', DriveCmd)
pub_status = rospy.Publisher('status', String)
def nearestNeighbor_inPath((x,y,th),currentindex_inPath):
"""
Given x,y,th, find the point closest to the robot in the path
"""
global path
d_min = float('inf')
index_min = 0
index = currentindex_inPath
for path_element in path[currentindex_inPath:currentindex_inPath+10]:
"""
Search only from the currentindex to currentindex+10
"""
d = util.distance_Euclidean(path_element.pose.x,path_element.pose.y,x,y)
if(d < d_min):
d_min = d
index_min = index
elif(d < 0.05): # if error is less than 5 cm, take this as the nearest point
d_min = d
index_min = index
break
index +=1
return index_min
def controller_PD():
"""
Implements controller
"""
"""
contains hardcoded debug paths to test the controller
specifically, has straightline and figure of eight
"""
import math
import parameters
import util
import lattice_planner
def straightLine((x1, y1, th1, v1), (x2, y2, th2, v2)):
path = []
dist = util.distance_Euclidean(x1, y1, x2, y2)
state = (x1, y1, th1, v1)
d = 0
while d <= dist:
newstate = util.go_Straight(state, 'f', 5)
path.append((newstate[0] / 100, newstate[1] / 100, newstate[2]))
d += 5
state = newstate
#print "appended",d,"of",dist
path_to_send = LatticePlannersim.Path()
for point in path:
pose = LatticePlannersim.Pose()
# plan uses center of car, so transform such that path has points to be traversed by rear axle center, # which is 17.41 cm away from the center
pose.x = point[0] - 17.41 * math.cos(point[2]) / 100.0
pose.y = point[1] - 17.41 * math.sin(point[2]) / 100.0
pose.theta = point[2]
pathelement = LatticePlannersim.PathElement()
"""
contains hardcoded debug paths to test the controller
specifically, has straightline and figure of eight
"""
import math
import parameters
import util
import lattice_planner
def straightLine((x1,y1,th1,v1),(x2,y2,th2,v2)):
path = []
dist = util.distance_Euclidean(x1,y1,x2,y2)
state = (x1,y1,th1,v1)
d = 0
while d <= dist:
newstate = util.go_Straight(state,'f',5)
path.append((newstate[0]/100,newstate[1]/100,newstate[2]))
d += 5
state = newstate
#print "appended",d,"of",dist
path_to_send = LatticePlannersim.Path()
for point in path:
pose = LatticePlannersim.Pose()
# plan uses center of car, so transform such that path has points to be traversed by rear axle center, # which is 17.41 cm away from the center
pose.x = point[0] - 17.41*math.cos(point[2])/100.0
pose.y = point[1] - 17.41*math.sin(point[2])/100.0
pose.theta = point[2]
pathelement = LatticePlannersim.PathElement()
pathelement.pose = pose
