Пример #1
0
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)