【ROS机器人运动控制激光雷达数据获取激光雷达避障篇——C++实现】

提示：环境配置为Ubuntu20.04&ROS Noetic

一、机器人运动理论部分

1.矢量运动和旋转运动

可以将机器人的运动拆解为矢量运动和旋转运动；
此外所有的运动都可以拆解为矢量运动和旋转运动；
机器人的中心点的定义——底盘到地面投影的中心点；

2.矢量运动方向

1.矢量运动的方向

按照右手坐标系，规定X轴、Y轴、Z轴正方向为：食指（X轴）、中指（Y轴）、大拇指（Z轴）

2.旋转运动的方向

按照右手螺旋定则，右手拇指沿坐标轴正方向，四指旋转方向规定为正方向

3.速度的量纲规范：（国际单位）

矢量运动速度大小 m/s
旋转运动速度大小 rad/s

二、使用C++实现机器人运动控制

1.程序设计思路

1.构建一个新的软件包，包名取为vel_pkg;
2.在软件中新建一个节点，节点名称叫做vel_node.cpp;
3.在节点中，向ROS的rospy申请发布话题/cmd_vel,并拿到发布对象vel_pub;
4.构建一个geometry_msgs/Twist类型的消息包vel_msg,用来承载要发送的速度值；
5.开启一个while循环，不停使用vel_pub发布对象发送速度消息包vel_msg;

2.具体实现步骤

1.在catkin_ws/src/目录下创建一个vel_pkg的包

cd catkin_ws/src/
catkin_create_pkg vel_pkg roscpp rospy geometry_msgs
注意这里多了一个依赖项geometry_msgs

2.在vscode的vel_pkg的src子目录下创建vel_node.cpp文件

编写代码

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>


int main(int argc,char *argv[])
{
    ros::init(argc,argv,"vel_node");//初始化节点
    printf("This is vel_node\n");

    ros::NodeHandle nh;//在节点初始化完成后定义一个NodeHandle对象
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);//定义一个发布者对象，发布的数据类型TWist，话题名称cmd_vel
    geometry_msgs::Twist vel_msg;//声明一个twist类型的消息包vel_msg用来承载要发送的速度值
    
    vel_msg.linear.x = 0.1;//对消息包的速度进行赋值，twist包含linear矢量速度和angular旋转速度
    vel_msg.linear.y = 0;
    vel_msg.linear.z = 0;
    
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 0.1;

    ros::Rate vel_node_pub_rate(30);//每秒钟30次
    
    while(ros::ok())
    {
        vel_pub.publish(vel_msg);
        vel_node_pub_rate.sleep();
    }
    return 0;
}

3.在CMakeLists.txt文件中添加指令

add_executable(vel_node src/vel_node.cpp)
target_link_libraries(vel_node
  ${catkin_LIBRARIES}
)

4.编译运行

CTRL+SHIFT+B

5.运行仿真环境wpr_simulation

roslaunch wpr_simulation wpb_simple.launch

6.运行vel_node节点

CTRL+SHIF+O
rosrun vel_pkg vel_node

3.实验现象

红色(X轴)，蓝色(Z轴)，绿色（Y轴），参照右手系；
机器人向X轴的正方向以0.1m/s的速度进行运动；

修改z轴的旋转速度0.1rad/s，右手螺旋准则——四指的方向为机器人旋转的方向；

三、激光雷达的数据分析

1.获取激光雷达的数据

启动ros
roscore

运行wpr_simulation中的wpb_simple.launch的程序示例
roslaunch wpr_simulation wpb_simple.launch

运行rviz
roslaunch wpr_simulation wpb_rviz.launch

激光雷达的数据：

2.雷达表达消息的格式

index.ros.org
搜索sensor_msgs
找到noetic版本
进入Website网址
在消息类型的列表中找到LaserScan
打开可以查看激光雷达消息包的格式定义

消息包的成员变量
成员变量的数据类型
rostopic echo /scan --noarr
其中–noarr表示把数据折叠起来，避免刷屏

3.雷达数据包截取

---
header: 
  seq: 815
  stamp: 
    secs: 217
    nsecs: 681000000
  frame_id: "laser"
angle_min: -3.141590118408203
angle_max: 3.141590118408203
angle_increment: 0.017501894384622574
time_increment: 0.0
scan_time: 0.0
range_min: 0.23999999463558197
range_max: 6.0
ranges: "<array type: float32, length: 360>"
intensities: "<array type: float32, length: 360>"
---

4.雷达数据解释说明

header的stamp表示时间戳，表示本次扫描中接收到第一束激光测距激光反射信号的时间；
header中的frame_id指明了激光雷达的基准坐标系；
后续内容关于角度方向的描述，都以这个坐标系为基准；
header:

angle_min: -3.141590118408203
angle_max: 3.141590118408203
表示扫描两次测距的旋转夹角（单位：弧度）

angle_increment: 0.017501894384622574
表示两次测距行为之间的角度间隔和时间间隔；

time_increment: 0.0
两次测距的时间差；
这两个参数在无人机、赛车上用的多，主要用来修正运动过程中雷达测距点阵的形变；
对于低速运动的机器人来说可以不考虑这个形变的影响；

scan_time: 0.0
两次扫描之间的时间间隔；
单次扫描是雷达转一周所耗费的时间；

ranges: “<array type: float32, length: 360>”
雷达在扫描的过程中每旋转1度就进行一次测距行为；
range这里面360个测距值对应的每一度的障碍物距离；
这里的32位浮点数数组里存放的就是测距值，单位米；

intensities: “<array type: float32, length: 360>”
所有测距返回信号强度（单位由厂商自己定义）
数值越大表示这次测距的信号强度越强；

四、获取激光雷达数据的C++节点

1.编译开源工程wpr_simulation

进入wpr_simulation的工程文件包
cd ~/catkin_ws/
编译
catkin_make

2.运行例子程序查看效果：

roslaunch wpr_simulation wpb_simple.launch
rosrun wpr_simulation demo_lidar_data

3.雷达消息包与雷达数据话题/scan

在ros系统中激光雷达通常会有一个对应的节点，这个节点一般由厂商提供，只需要简单配置一下端口参数即可和激光雷达的电路系统建立连接；
雷达的测距数值从电路系统传输到雷达节点，然后被封装成一个消息包并且发布在一个Topic话题中；
订阅上述话题即可获取激光雷达的数据；
消息包的格式即LaserScan格式，即sensor_msgs::LaserScan ;
雷达数据话题为 /scan ；

4.程序设计思路

1.构建一个新的软件包，包名称为lidar_pkg
2.在软件包中构建一个节点，节点名称为lidar_node.cpp
3.在节点中，向ros中的NodeHandle申请订阅话题/scan，并设置回调函数为LidarCallback()
4.构建回调函数LidarCallback()用于接收和处理雷达数据；
5.调用ROS_INFO()显示雷达检测到的前方障碍物距离；

5.具体操作步骤

1.创建lidar_pkg包

cd catkin_ws/src/
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
注意此处的依赖项多了一个sensor_msgs

2.在vscode的lidar_pkg目录下的src自文件夹下创建liar_node.cpp文件

3.编写liar_node.cpp文件

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

void lidar_callback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];//msg的数组里存储了360个测距值
    ROS_INFO("Front distance is ranges[180] = %f m",fMidDist);
}

int main(int argc,char *argv[])
{
    setlocate(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");//初始化节点
    printf("This is lidar_node\n");

    ros::NodeHandle nh;//在节点初始化完成后定义一个NodeHandle对象
    ros::Subscriber sub = nh.subscribe("/scan",10,lidar_callback);//设置订阅的话题、消息缓存长度、回调函数名
    
    ros::spin();
    return 0;
}

4.编写CMakeLists.txt文件

add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node
  ${catkin_LIBRARIES}
)

6.运行节点测试效果

1.启动ros

CTR+ALT+T
roscore

2.启动wpr_simulation

CTR+SHIFT+O
roslaunch wpr_simulation wpb_simple.launch

3.启动 lidar_node 节点

CTR+SHIFT+O
rosrun lidar_pkg lidar_node

4.实验现象

lidar_node 节点订阅了/scan话题并成功打印出了接收到的机器人正前方的雷达测距数据；

7.常用测试指令

查看话题列表
rostopic list

查看对应话题的名称
查看/scan话题中的消息
rostopic echo /scan

查看节点间通信关系
rqt_graph

运行rviz
roslaunch wpr_simulation wpb_rviz.launch

五、利用C++实现激光雷达的简易避障控制

避障思路
1.让NodeHandle发布速度控制话题/cmd_vel
2.构建速度控制消息包vel_cmd
3.根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物；

在lidar_node.cpp中编写代码

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>//引入雷达消息的头文件
#include <geometry_msgs/Twist.h>//引入速度消息的头文件

ros::Publisher vel_pub ;//发布者对象

/*lidar_node节点订阅/scan话题的同时会发布/cmd_vel话题*/

void lidar_callback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];//msg的数组里存储了360个测距值
    ROS_INFO("Front distance is ranges[180] = %f m",fMidDist);

    geometry_msgs::Twist vel_cmd;//在回调函数里生成一个速度控制消息包vel_cmd
    if(fMidDist <1.5)//根据激光雷达的测距数值实时调整机器人的运动速度，避开障碍物
    {
        vel_cmd.angular.z = 0.5;//+左转，转速单位rad/s
    }
    else
    {
         vel_cmd.linear.x = 0.1;//+X轴正方向，速度单位m/s
    }
    vel_pub.publish(vel_cmd);//发布速度话题
}

int main(int argc,char *argv[])
{
    ros::init(argc,argv,"lidar_node");//初始化节点
    printf("This is lidar_node\n");

    ros::NodeHandle nh;//在节点初始化完成后定义一个NodeHandle对象
    ros::Subscriber scan_sub = nh.subscribe("/scan",10,lidar_callback);//设置订阅激光雷达的话题/scan、消息缓存长度、回调函数名

    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);//发布速度话题/cmd_vel，控制电机的运转

    ros::spin();

    while(ros::ok())
    {

    }
    return 0;
}

启动ros
CTR+ALT+T
roscore

启动wpr_simulation
CTR+SHIFT+O
roslaunch wpr_simulation wpb_simple.launch

启动 lidar_node 节点
CTR+SHIFT+O
rosrun lidar_pkg lidar_node

运行rviz
CTR+SHIFT+O
roslaunch wpr_simulation wpb_rviz.launch

查看话题列表
rostopic list

查看对应话题的名称
查看/scan话题中的消息
rostopic echo /scan

查看节点间通信关系
rqt_graph

机器人运行结果与分析
机器人正前方在距离障碍物1.5m时向左侧以0.5rad/s的设定速度旋转；

lidar_node节点通信关系分析

总结

实现了C++程序对机器人运动的控制；
简单分析了激光雷达的数据包构成；
使用C++实现了激光雷达/scan话题的订阅者节点；
使用C++实现了简易的激光雷达避障功能；