🎬 渡水无言 ：个人主页渡水无言
❄专栏传送门 ： 《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》
❄专栏传送门 ： 《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》
❄专栏传送门 ：《产品测评专栏》 《Ai智能体专栏） 《ROS开发专栏》
⭐️流水不争先，争的是滔滔不绝
📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生
| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生
在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连
目录
[3.3、package.xml 依赖](#3.3、package.xml 依赖)
总结
前言
上一期博客我们实现了激光雷达数据读取的实验，本期博客我们本章我们将雷达数据 + 运动控制结合，实现机器人自动避障。为后续 SLAM 建图、自主导航打下基础。
一、避障原理
程序订阅雷达数据的"/scan"话题，从中接收激光雷达节点发来的sensor_msgs::LaserScan类型消息包，并解析出雷达测距值。根据雷达测距值让机器人做出反应：前方没有障碍物就前进，前方遇到障碍物就转向。如下图所示：
二、避障程序流程框图
订阅雷达数据：获取 /scan 话题的激光雷达数据，解析正前方障碍物距离
判断障碍物：
若前方距离 > 安全阈值（1.5m）→ 机器人保持前进
若前方距离 < 安全阈值 → 机器人原地左转，避开障碍物
发布速度指令：通过 /cmd_vel 话题控制机器人运动，实现 "遇障就转、无障就走"
三、创建避障节点的代码实现
我们直接在上一期的博客创建 lidar_pkg功能包中新增避障节点，无需新建包。
3.1、编写避障节点代码
在 lidar_pkg/src/ 目录下新建 lidar_avoid.cpp 文件，编写代码如下所示：
cpp
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>
// 全局节点指针
std::shared_ptr<rclcpp::Node> node;
// 速度发布器
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;
// 延时计数器，防止频繁抖动
int nCount = 0;
// 雷达数据回调函数
void LidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
// 获取雷达数据总数
int nNum = msg->ranges.size();
// 正前方索引
int nMid = nNum / 2;
// 正前方距离
float fMidDist = msg->ranges[nMid];
// 打印信息
RCLCPP_INFO(node->get_logger(), "ranges[%d] = %f m", nMid, fMidDist);
// 延时处理
if (nCount > 0)
{
nCount--;
return;
}
// 定义速度消息
geometry_msgs::msg::Twist vel_msg;
// 避障判断
if (fMidDist < 1.5f)
{
// 左转避障
vel_msg.angular.z = 0.3;
nCount = 100;
}
else
{
// 直行
vel_msg.linear.x = 0.1;
}
// 发布速度
vel_pub->publish(vel_msg);
}
int main(int argc, char** argv)
{
// 初始化ROS2
rclcpp::init(argc, argv);
// 创建节点
node = std::make_shared<rclcpp::Node>("lidar_behavior_node");
// 创建速度发布者
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
// 创建雷达订阅者
auto lidar_sub = node->create_subscription<sensor_msgs::msg::LaserScan>(
"/scan", 10, LidarCallback);
// 循环等待回调
rclcpp::spin(node);
// 关闭ROS2
rclcpp::shutdown();
return 0;
}3.2、编写CMakeLists.txt
编写代码如下：
cpp
cmake_minimum_required(VERSION 3.8)
project(lidar_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
# 编译雷达数据获取节点
add_executable(lidar_data src/lidar_data.cpp)
ament_target_dependencies(lidar_data
rclcpp
sensor_msgs
)
# 编译避障节点
add_executable(lidar_behavior src/lidar_behavior.cpp)
ament_target_dependencies(lidar_behavior
rclcpp
sensor_msgs
geometry_msgs
)
# 安装节点
install(TARGETS
lidar_data
lidar_behavior
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()3.3、package.xml 依赖
添加一行<depend>geometry_msgs</depend>，全部代码如下所示：
cpp
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidar_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="duan@todo.todo">duan</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>四、编译与运行
编译功能包并运行仿真程序
cpp
cd ~/ros2_ws
colcon build
source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py打开第二个终端
运行避障节点
cpp
ros2 run lidar_pkg lidar_behavior节点运行起来之后，切换到刚才的仿真窗口。如图所示，可以看到机器人先以0.1米/秒的速度向前移动。当机器人距离书柜1.5米时，机器人停止前进，开始向左逆时针旋转。旋转一段时间后，机器人恢复直行，顺利的避开前方的书柜。
总结
本期博客完成了 ROS2 激光雷达自动避障 的完整实现，通过订阅 /scan 雷达数据、解析障碍物距离、结合 /cmd_vel 速度控制，实现了一套简单可靠的自主避障系统。