ROS实现简单避障

        通过ros中的topic话题通讯机制，依靠速度话题/cmd_vel和激光雷达的测距反馈，实现避开障碍物。

#!/usr/bin/env python3
# coding=utf-8

import rospy
from sensor_msgs.msg import LaserScan
# 传感器消息类型 LaserScan翻译为激光扫描
from geometry_msgs.msg import Twist


# 几何消息类型 geometry翻译为几何 Twist翻译为转动、扭转


def LidarCallback(message):
    # 声明全局变量，用来发送速度的消息，控制消息
    global velocity_publisher
    # count变量用来控制避障时，机器人转向的时间
    global count
    # 得到机器人正前方的障碍物距离，并输出
    distance = message.ranges[180]
    rospy.loginfo("距离前方障碍物还有%.2fm", distance)

    # 此刻是避障调整代码执行，只调整角度，调整完了直接推出函数，不会发布速度，所以说是静止调整好了角度再运行的
    if count > 0:
        count = count - 1
        return
    # 创建速度控制的消息
    velocity_command = Twist()
    # 如果距离前方障碍物距离小于1.5m，则给count赋值为35，激光雷达的扫描频率大概是11hz，相当于角度调整了3.5s左右
    # 调整的是旋转的角度以避开障碍，0.3rad/s
    if distance < 1.5:
        velocity_command.angular.z = 0.3
        count = 35
    # 如果没有到达避障的位置，则正常的向前方以0.15m/s的速度直行
    else:
        velocity_command.linear.x = 0.15
    # 不管是调整还是正常直行，都需要发布速度给机器人，来控制速度
    velocity_publisher.publish(velocity_command)


if __name__ == '__main__':
    # 初始化ros节点并发出警告，表明节点开始运行
    rospy.init_node("easy_avoid_node")
    rospy.logwarn("easy_avoid_node start running")
    # 创建激光雷达的订阅者对象
    lidar_subscriber = rospy.Subscriber("/scan", LaserScan, LidarCallback, queue_size=10)
    # 创建速度发布者对象
    count = 0
    velocity_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
    # 控制节点，一直运行
    rospy.spin()