control.py 文件源码

def compute_control_actions():
    global i
    controller.compute_control_actions(current_pose, current_twist, i)
    data_container['t'].append(i * DELTA_T)
    data_container['x'].append(current_pose.position.x)
    data_container['x_ref'].append(controller.x_ref_n)
    data_container['y'].append(current_pose.position.y)
    data_container['y_ref'].append(controller.y_ref_n)
    data_container['theta'].append(controller.theta_n)
    data_container['theta_ref'].append(controller.theta_ref_n)
    data_container['v_c'].append(controller.v_c_n)
    data_container['w_c'].append(controller.w_c_n)
    data_container['zeros'].append(0)

    twist = Twist()
    twist.linear.x = controller.v_c_n
    twist.angular.z = controller.w_c_n
    twist_publisher.publish(twist)

    i += 1