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