def generate_final_course(self, goal_ind):
path = points()
path.x = [self.end.x]
path.y = [self.end.y]
node = self.node_list[goal_ind]
while node.parent is not None:
path.x.append(node.x)
path.y.append(node.y)
node = node.parent
path.x.append(node.x)
path.y.append(node.y)
path.x.reverse()
path.y.reverse()
del path.x[0]
del path.y[0]
print("--------------")
print(path.x)
print(path.y)
print("..............")
return path
def __init__(self, start, goal, obstacle_list, rand_area, radius,
expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, max_iter=500):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y],...]
randArea:Random Sampling Area [min,max]
"""
self.start = self.Node(start[0], start[1]) #Start Position
self.end = self.Node(goal[0], goal[1]) #End/Goal Position
self.min_rand = rand_area[0] #randArea : Random Sampling Area [min_rand, max_rand]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = []
self.path = points()
self.radius = radius
def __init__(self, current_time=None):
rospy.init_node('pid_controller', anonymous = True)
self.x = 0.0
self.y = 0.0
self.flag = False # flag value will be used for switching between rotation and translation
self.theta = 0.0
self.roll = 0.0
self.pitch = 0.0
self.goal_points = Path()
self.goal = Point()
self.new_x = self.goal.x - self.x # this will be an input to actuator response
self.new_y = self.goal.y - self.y # this will be an input to actuator response
self.n_points = 0 # number of points in the path Z
self.n = points()
self.n.x = [0.0]
self.n.y = [0.0]
self.sub = rospy.Subscriber('/path', points, self.get_path)
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.get_odom)
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)
self.rate = rospy.Rate(50) # 50 Hz
self.Kp = 1.0 # Default Proportionality Constant
self.Ki = 0.0 # Default Integral Constant
self.Kd = 0.0 # Default Derivative Constant
self.feedback_value = 0.0 # This will help us in controlling the actuator response
self.SetPoint = 100 # We will compare our feedback_value to the SetPoint value
self.current_time = current_time if current_time is not None else time.time() # We will use time difference of short intervals to calculate Integral and Derivative Terms
self.last_time = self.current_time # After each iteration we will update the last_time and current time
self.PTerm = 0.0 # Default Proportionality Term
self.ITerm = 0.0 # Default Integral Term
self.DTerm = 0.0 # Default Derivative Term
self.last_error = 0.0 # After each iteration we will update the last_error.
self.error = 1000.0
# Windup Guard - In case the integral term accumulates a significant error during the rise (windup), thus overshooting.
self.windup_guard = 1000000000.0
def __init__(self, x, y):
self.x = x
self.y = y
self.path = points()
self.parent = None