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)