前言

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

在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

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

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

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

十五、重新运行节点

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