ROS 基于激光雷达的机器人避障实现

使用编辑器在 ~/catkin_ws/src/lidar_pkg/src 目录下创建文件：lidar_node.cpp

编辑新建的节点文件，输入以下内容并保存。

#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米", fMidDist);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "lidar_node");
    ros::NodeHandle nh;
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &LidarCallback);
    ros::spin();
    return 0;
}

(3) 设置 CMakeLists.txt 文件

在与节点同一地址中打开 CMakeLists.txt 文件，将下方内容复制黏贴到末尾。

add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node ${catkin_LIBRARIES})

(4) 编译

cd ~/catkin_ws/
catkin_make

2、运行新编写的 lidar_node.cpp 节点，查看订阅到的激光雷达数据信息

roslaunch wpr_simulation wpb_simple.launch
# 在新的终端运行
rosrun lidar_pkg lidar_node

也可在 Gazebo 中移动机器人或者障碍物的位置观察输出的距离。

四、将获取的激光雷达数据发布出去，给速度控制节点实现机器人的避障

（1）修改 lidar_node 节点

打开 VScode 软件在 ~/catkin_ws/src/lidar_pkg/src 目录下的文件：lidar_node.cpp

修改其中内容：

#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>

ros::Publisher vel_pub;
int nCont = 0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米", fMidDist);

    geometry_msgs::Twist vel_cmd;
    if(nCont > 0)
    {
        nCont--;
        return;
    }

    if(fMidDist < 1.5)
    {
        vel_cmd.angular.z = 0.5;
        nCont = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.5;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "lidar_node");
    ros::NodeHandle nh;
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &LidarCallback);
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::spin();
    return 0;
}

（2）编译

cd ~/catkin_ws
catkin_make

(3) 运行新节点（关闭之前打开的 Gazebo 仿真）

roslaunch wpr_simulation wpb_simple.launch
rosrun lidar_pkg lidar_node

结果

机器人直行到距离前方障碍物 1.5 米时，绕 Z 轴转向，继续直行，绕开障碍物。