ROS2-第五章 ROS2机器人运动控制 陶楚沅版教参
激光雷达在ROS2中的使用
5.1 ROS2里的激光雷达数据
激光雷达的旋转部分搭载激光测距探头，其在旋转的过程中每隔一定角度就会测量一次距离值。当其旋转一圈时，刚好可以对周围障碍物进行均匀的测距采样，这么一组测距值就是激光雷达的输出数值。
消息类型
ROS2中的激光雷达的节点会在指定名称的话题里发布消息。话题名称在REP-138中推荐为“/scan”。消息的类型为sensor_msgs::LaserScan，其格式如图所示：
在速度控制中，我们的发布者节点也是定义了类似的消息格式的
适用的时候要提前定义：
geometry_msgs::msg::Twist vel_msg;
vel_msg.linear.x = 0.1;
vel_msg.linear.y = 0.0;
vel_msg.linear.z = 0.0;
vel_msg.angular.x = 0.0;
vel_msg.angular.y = 0.0;
vel_msg.angular.z = 0.0;
5.2 在RViz2中查看激光雷达数据
在ROS2中，提供了一个图形化工具，可以直观的查看传感器的数值。这个工具名字叫“RViz2”，其中“RViz”是“ROS Visualization Tool”的缩写，“2”表示第二代。
保持之前的WPR仿真机器人窗口不关。
开一个新终端（ctrl+ALT+T）
cd ros2_ws
source ~/ros2_ws/install/setup.bash
rviz2
打开后，选择坐标系为base_footprint
然后现在画面上啥也没有，需要添加机器人和激光雷达
找到左下角add
添加robotmodel
现在还是啥也没有，别着急，点击这个Descriptiontopic
选择这个
出现啦！
然后再点击Add，把Laserscan点出来
一开始也是禁用状态，你必须为他添加话题，点击Topic
这个点可能不是很明显
可以把这个size调大一些，就能看清楚1
看到这样的画面就是成功了
你可以点击File保存为XXX.rviz，下次就能rviz2 -d ~/XXX.rviz启动了
5.3 激光雷达数据获取（订阅者节点编写）
激光雷达数据的获取是通过订阅激光雷达发布的话题，从话题中获取激光雷达发出的消息包来实现的。如图所示，将会实现一个订阅者节点，订阅"/scan"话题。从此话题中接收sensor_msgs::LaserScan类型的消息包，并解析出雷达的测距数值。
同理，先建包
cd ~/ros2_ws/src
ros2 pkg create lidar_pkg
在包内的src中创建lidar_data.cpp，复制以下内容
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
std::shared_ptr<rclcpp::Node> node;
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);
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("lidar_data_node");
auto lidar_sub = node->create_subscription<sensor_msgs::msg::LaserScan>("/scan", 10, LidarCallback);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
修改MakeList
在第一个实验中我们已经了解，所有的include都需要在MakeList中的find_package添加。
引入了sensor_msgs::Twist，因此要
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
同时要添加这个ament_target，才能自动链接这两个库。
这两步就是先找到库在哪里，再链接上用到这两个库的文件。
ament_target_dependencies(lidar_data "rclcpp" "sensor_msgs")
然后需要创建该节点的可执行文件exe，命名为publisher_node，引用src下的publisher_node.cpp
add_executable(lidar_data src/lidar_data.cpp)
接着要把上一步这个可执行文件（节点Node）安装到指定文件夹（package）下。这个文件夹ros2是能够访问的，这样才能通过ros2去启动他。
install(TARGETS lidar_data
DESTINATION lib/${PROJECT_NAME})
完整Cmakelist
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 dependencies
find_package(ament_cmake REQUIRED)
# ==================== 新增依赖 ====================
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
# ==================================================
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
# ==================== 新增可执行文件配置 ====================
add_executable(lidar_data src/lidar_data.cpp)
ament_target_dependencies(lidar_data "rclcpp" "sensor_msgs")
install(TARGETS lidar_data
DESTINATION lib/${PROJECT_NAME})
# ==========================================================
ament_package()
修改package.xml
还记得第一个示例的“注意”吧？
CMakeLists.txt 里的 find_package 是给编译器看的；
package.xml 里的 <depend> 是给包管理器（rosdep）和构建系统（colcon）看的。两者缺一不可，必须保持同步。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
完整package.xml：
<?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="unicorn@todo.todo">unicorn</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_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>
在ros2_ws colcon build一下
colcon build
保持之前的wpr别关，关了就新开一个终端
source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py
在之前colconbuild的终端source一下
source install/setup.bash
执行
ros2 run lidar_pkg lidar_data
可以看到数据源源不断传入了
5.4 基于激光雷达的避障实现（发布者-订阅者闭环）
下面将激光雷达数据获取和速度控制进行结合，实现一个避障程序。程序订阅雷达数据的“/scan”话题，从中接收激光雷达节点发来的sensor_msgs::LaserScan类型消息包，并解析出雷达测距值。根据雷达测距值让机器人做出反应：前方没有障碍物就前进，前方遇到障碍物就转向。
在lidar_data.cpp下面新建lidar_behaviour.cpp
很好理解，main中创建pub和sub两个节点，注意由于pub需要发布速度消息包的，而发布速度是要在接收到激光雷达后通过回调函数触发的，因此要在外声明。
然后LidarCallback中处理避障逻辑。
#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)
{
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);
rclcpp::shutdown();
return 0;
}
每收到一帧激光雷达数据，ROS 2 就会自动调用这个函数。
int nNum = msg->ranges.size();
int nMid = nNum / 2;
float fMidDist = msg->ranges[nMid];
-
ranges是激光雷达各个角度的测距数组，单位是米。 -
nMid = nNum / 2取数组中间的下标，一般对应雷达的正前方（前提是雷达扫描角度对称）。 -
fMidDist就是正前方障碍物的距离。
RCLCPP_INFO(node->get_logger(), "ranges[%d] = %f m", nMid, fMidDist);
打印正前方的距离，方便调试。
if (nCount > 0)
{
nCount--;
return;
}
这是一个冷却计数器。如果 nCount > 0，就减 1 并直接返回，不发布任何速度指令。
它的作用是：当机器人决定转向后，让后续 100 次雷达回调都“跳过控制”，相当于保持上一次发布的转向指令一段时间。
geometry_msgs::msg::Twist vel_msg;
创建一个速度指令消息，包含线速度 linear 和角速度 angular。
if (fMidDist < 1.5f)
{
vel_msg.angular.z = 0.3;
nCount = 100;
}
如果正前方距离小于 1.5 米，说明有障碍物：
-
设置角速度
angular.z = 0.3 rad/s（原地左转或右转，取决于正负号，这里是正数）。 -
设置
nCount = 100，表示接下来 100 次雷达回调都跳过控制，让机器人持续转向一段时间。
else
{
vel_msg.linear.x = 0.1;
}
如果前方没有障碍物：
-
设置线速度
linear.x = 0.1 m/s，让机器人缓慢前进。 -
角速度默认为 0，所以是直行。
vel_pub->publish(vel_msg);
把速度指令发布出去，底盘收到后就会执行。
整体逻辑
收到雷达数据
↓
取正前方距离
↓
如果正在冷却（nCount > 0），跳过本次控制
↓
如果正前方 < 1.5m：
发布角速度 0.3，并设置冷却 100 次
否则：
发布线速度 0.1
修改MakeList
在第一个实验中我们已经了解，所有的include都需要在MakeList中的find_package添加。
引入了sensor_msgs::Twist，因此要
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
同时要添加这个ament_target，才能自动链接这两个库。
这两步就是先找到库在哪里，再链接上用到这两个库的文件。
ament_target_dependencies(lidar_behavior "rclcpp" "sensor_msgs" "geometry_msgs")
然后需要创建该节点的可执行文件exe，命名为publisher_node，引用src下的publisher_node.cpp
add_executable(lidar_behavior src/lidar_behavior.cpp)
接着要把上一步这个可执行文件（节点Node）安装到指定文件夹（package）下。这个文件夹ros2是能够访问的，这样才能通过ros2去启动他。
install(TARGETS lidar_data lidar_behavior
DESTINATION lib/${PROJECT_NAME})
完整Cmakelist
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 dependencies
find_package(ament_cmake REQUIRED)
# ==================== 依赖项配置 ====================
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED) # <--- 新增 geometry_msgs
# ====================================================
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()
# ==================== 可执行文件配置 ====================
# 1. 原有的 lidar_data 节点
add_executable(lidar_data src/lidar_data.cpp)
ament_target_dependencies(lidar_data "rclcpp" "sensor_msgs")
# 2. 新增的 lidar_behavior 节点
add_executable(lidar_behavior src/lidar_behavior.cpp)
ament_target_dependencies(lidar_behavior "rclcpp" "sensor_msgs" "geometry_msgs")
# ==================== 安装规则 ====================
# 将两个可执行文件合并到同一个 install 指令中
install(TARGETS
lidar_data
lidar_behavior
DESTINATION lib/${PROJECT_NAME})
# ====================================================
ament_package()
修改package.xml
还记得第一个示例的“注意”吧？
CMakeLists.txt 里的 find_package 是给编译器看的；
package.xml 里的 <depend> 是给包管理器（rosdep）和构建系统（colcon）看的。两者缺一不可，必须保持同步。
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
完整package.xml：
<?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="unicorn@todo.todo">unicorn</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>
在ros2_ws colcon build一下
colcon build
保持之前的wpr别关，关了就新开一个终端
source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py
在之前colconbuild的终端source一下
source install/setup.bash
执行
ros2 run lidar_pkg lidar_behavior
可以看到小车避开了橱柜：
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
4
0- 0
已为社区贡献24条内容
所有评论(0)