def control(data):
msg = drive_param()
global prev_error
global velocity
global kp
global kd
global flag
global length
global path_y
global path_x
myrobot = robot()
myrobot.set(data.pose.position.x,
data.pose.position.y) #update current pose
''' flag is set when path arrives from planner node'''
if flag == 1:
'''find distance from robot and the desired track (cross track error)'''
for index in range(0, ((length / 2) - 1)):
myrobot.set(data.pose.position.x,
data.pose.position.y) #update current pose
print("curr_y = ", myrobot.y, "curr_x = ", myrobot.x)
print("path_y = ", path_y[index], "path_x = ", path_x[index])
p1 = Point(path_y[index],
path_x[index]) # points from desired path
p2 = Point(path_y[index + 1], path_x[index + 1])
s = Segment(p1, p2)
error = -float(s.distance(
myrobot.curr_pose)) # x in the equation --> -x-Lsin(theta)
if (myrobot.y < path_y[index]):
error = -error #-error = left turn, +ve error = right turn
print("error = ", error)
'''calculate steering angle using pid controller'''
#int_error += error # integral not used
diff_error = error - prev_error # differential
angle = -(kp * error) - (kd * diff_error) # - (ki * int_CTE)
if angle < -100:
angle = -100
elif angle > 100:
angle = 100
prev_error = error
msg.angle = angle
velocity = 10.0
msg.velocity = velocity
#print("flag = 1, vel = " , msg.velocity, ", angle = " , msg.angle)
pub.publish(msg)
flag = 0
''' keep vel and steering to 0 if no plan arrives and flag is not set'''
angle = 0
velocity = 0.0
msg.angle = angle
msg.velocity = velocity # constant velocity
print("flag = 0, vel = ", msg.velocity, ", angle = ", msg.angle)
pub.publish(msg)
