ROS学习记录：在ROS中用C++实现激光雷达避障

前言

本文建立在成功获取激光雷达数据的基础上，详细参考

在ROS中用C++实现获取激光雷达的数据

一、实现思路

二、在VScode中打开之前编写好的lidar_node.cpp

三、在lidar_node.cpp中写入如下代码

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

ros::Publisher vel_pub;
static int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    
    int nMid = nNum/2;
    float fMidDist = msg.ranges[nMid];
    ROS_INFO("前方测距 ranges[%d] = %f 米", nMid, fMidDist); 

    if(nCount > 0)
    {
        nCount--;
        return;
    }

    geometry_msgs::Twist vel_cmd;
    if(fMidDist < 1.5f)
    {
        vel_cmd.angular.z = 0.3;
        nCount = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.05;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"demo_lidar_behavior");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
}

四、保存后，输入Shift+Ctrl+B进行编译，选择catkin_make:build

五、编译成功

六、打开一个终端，输入cd ~/catkin_ws1进入工作空间

七、输入source ./devel/setup.bash

八、输入roslaunch wpr_simulation wpb_simple.launch打开机器人仿真环境

九、机器人仿真环境打开成功

十、再打开一个工作空间终端，输入source ./devel/setup.bash

十一、输入rosrun lidar_pkg lidar_node

十二、查看仿真环境中的机器人，可以看到机器人向前运动

十三、在机器人将要碰到书柜时，机器人成功转向再向前运动，避障成功

十四、给机器人添加上一些圆柱体障碍物，看它能不能也成功避障

十五、重新运行节点

十六、测试发现，机器人能够连续避障，不过只有当障碍物在机器人的正前方才会避障成功，因为在代码里我们只判断了机器人正前方的距离，机器人还是有可能会发生侧边碰撞，所以要想机器人做全方位避障，还需对代码进行进一步优化，需要对各方向的距离值做进一步处理