（11-4-01）基于SLAM的自主路径导航系统：路径规划(1)

11.5  路径规划

本项目的路径规划部分基于TurtleBot3机器人和ROS框架，利用SLAM技术实现了实时建图和定位，并结合move_base包实现了路径规划功能。通过传感器获取的环境信息，机器人在地图上进行自主导航，以达到指定目标点的目的。路径规划算法可以根据不同的SLAM方法进行选择，包括gmapping、cartographer、hector、karto、frontier_exploration等，用户可以根据需要选择合适的方法进行路径规划。整个项目通过ROS的launch文件进行配置和启动，可视化部分使用RViz工具实时展示机器人的运动轨迹和地图信息，为用户提供了一个方便直观的界面来监控和调试路径规划的效果。

11.5.1  跟墙壁行驶

文件wall_follow.py实现了一个名为wallfollowing_control的ROS节点，用于控制机器人跟随墙壁行驶。此文件订阅了激光雷达数据以检测周围环境，根据前方和侧面的距离误差使用PD控制器生成线速度和角速度指令，并通过`/cmd_vel`话题发布速度消息控制机器人移动。同时，节点会打印距离信息和控制指令以进行调试。

# 设定激光扫描的起始角度、范围和前方扫描范围以及与墙之间的距离
side_scanstartangle = 20
side_scanrange = 60
front_scanrange = 16
distancefromwall = 0.4

# 初始化存储激光数据的变量
x = np.zeros((360))
s_d = 0  # 前方墙壁距离
y_l = 0  # 左侧墙壁距离
y_r = 0  # 右侧墙壁距离

# PID参数
kp = 4
kd = 450
ki = 0

k1 = kp + ki + kd
k2 = -kp - 2 * kd
k3 = kp


def callback(data):
    global y_l, y_r, x, s_d, front_scanrange, side_scanstartangle, side_scanrange

    # 存储激光扫描数据
    x = list(data.ranges)
    for i in range(360):
        if x[i] == inf:
            x[i] = 7
        if x[i] == 0:
            x[i] = 6

    # 计算存储扫描数据
    y_l = min(x[side_scanstartangle:side_scanstartangle + side_scanrange])  # 左侧墙壁距离
    y_r = min(x[360 - side_scanstartangle - side_scanrange:360 - side_scanstartangle])  # 右侧墙壁距离
    s_d = min(min(x[0:int(front_scanrange / 2)], x[int(360 - front_scanrange / 2):360]))  # 前方墙壁距离


def tmnt_controller():
    # 初始化参数
    global k1, k2, k3, kp, kd, ki, s_d, x, y_r, y_l
    global distancefromwall

    # 初始化PID控制器参数
    prev_PID_output = 0
    prev_error = 0
    prev_prev_error = 0

    rospy.init_node('wallfollowing_control', anonymous=True)
    velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    scan_subscriber = rospy.Subscriber('/scan', LaserScan, callback)
    rate = rospy.Rate(10)  # 控制频率为10hz

    while not rospy.is_shutdown():
        # 计算距离误差
        delta = distancefromwall - y_r

        # PID控制
        PID_output = kp * delta + kd * (delta - prev_error)

        # 存储状态
        prev_error = delta
        prev_prev_error = prev_error
        prev_PID_output = PID_output

        # 限制PID输出
        angular_zvel = np.clip(PID_output, -1.2, 1.2)

        linear_vel = np.clip((s_d - 0.35), -0.1, 0.4)

        # 调试信息
        print('距离右侧墙壁距离(cm) =', format(int(y_r * 100)), '/', format(distancefromwall * 100))
        print('距离前方墙壁距离(cm) =', format(s_d * 100))
        print('线性速度=', format(linear_vel), ' 角速度=', format(angular_zvel))
        rospy.loginfo('\n')

        # 发布速度消息
        vel_msg = Twist(Vector3(linear_vel, 0, 0), Vector3(0, 0, angular_zvel))
        velocity_publisher.publish(vel_msg)
        rate.sleep()

    velocity_publisher.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)))
    print('Turtlebot停止')


if __name__ == '__main__':
    try:
        # 启动控制器
        tmnt_controller()
    except rospy.ROSInterruptException:
        pass