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