前言
本文建立在成功获取激光雷达数据的基础上,详细参考
在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
十二、查看仿真环境中的机器人,可以看到机器人向前运动
十三、在机器人将要碰到书柜时,机器人成功转向再向前运动,避障成功
十四、给机器人添加上一些圆柱体障碍物,看它能不能也成功避障
十五、重新运行节点
十六、测试发现,机器人能够连续避障,不过只有当障碍物在机器人的正前方才会避障成功,因为在代码里我们只判断了机器人正前方的距离,机器人还是有可能会发生侧边碰撞,所以要想机器人做全方位避障,还需对代码进行进一步优化,需要对各方向的距离值做进一步处理