12.激光雷达工作原理
激光雷达的作用是探照周围障碍物的距离，按照测量维度可以分为单线雷达和多线雷达。 按照测量原理可以分为三角测距雷达和TOF雷达。按照工作方式可以分为固态雷达和机械旋转雷达。
本次讲解以TOF雷达为例，雷达发射器发射激光遇到障碍物再将雷达返回回来，被雷达的接收器捕获。通过计时器测量激光发射和接收的时间间隔测算雷达与障碍物之间的距离。
12.1使用RViz观测传感器数据
分别打开两个终端运行以下两条命令：
roslaunch wpr_simulation wpb_simple.launch
rviz
在rviz页面进行如下设置：
Add添加RobotModel和LaserScan并配置。
rviz是接收传感器的图形化界面，既可以接收拟真环境的也可以接收现实传感器的数据并且不参与运算。
12.2 激光雷达消息包格式
12.3 获取激光雷达数据的节点
先看样例，在两个终端里分别输入
roslaunch wpr_simulation wpb_simple.launch
rosrun wpr_simulation demo_lidar_data
在工作目录里创建新软件包：
cd catkin_ws/src
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
打开vscode，创建lidar_node.cpp。
#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
void LidarCallback(const sensor_msgs::LaserScan msg)
{
float fMidDist = msg.ranges[180];//获取前方180度的测距值
ROS_INFO("前方测距 ranges[180]: %f 米", fMidDist);//打印前方测距值
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");//设置中文显示
ros::init(argc, argv, "lidar_node");//初始化节点
ros::NodeHandle nh;//创建节点句柄
ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &LidarCallback);//订阅激光雷达数据
ros::spin();//循环等待回调函数
return 0;
}
cmake编译。
add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node
${catkin_LIBRARIES}
)
启动ros仿真环境测试
roslaunch wpr_simulation wpb_simple.launch
rosrun lidar_pkg lidar_node
13. 在ROS中实现激光雷达避障
对src下的lidar_node.cpp进行编写：
#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>
ros::Publisher vel_pub;//定义速度指令发布器
int nCount = 0;//定义计数器
void LidarCallback(const sensor_msgs::LaserScan msg)
{
float fMidDist = msg.ranges[180];//获取前方180度的测距值
ROS_INFO("前方测距 ranges[180]: %f 米", fMidDist);//打印前方测距值
if (nCount > 0)//如果计数器大于0
{
nCount--;//计数器减1
return;//返回
}
geometry_msgs::Twist vel_cmd;//定义速度指令
if(fMidDist < 1.5)//如果前方距离小于0.5米
{
vel_cmd.linear.x = 0.0;//停止前进
vel_cmd.angular.z = 0.3;//向右转
ROS_INFO("前方障碍物，停止前进，向右转");//打印信息
nCount=50;
}
else
{
vel_cmd.linear.x = 0.5;//前进
vel_cmd.angular.z = 0.0;//不转向
ROS_INFO("前方无障碍物，继续前进");//打印信息
}
vel_pub.publish(vel_cmd);//发布速度指令
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");//设置中文显示
ros::init(argc, argv, "lidar_node");//初始化节点
ros::NodeHandle nh;//创建节点句柄
ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &LidarCallback);//订阅激光雷达数据
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);//发布速度指令
ros::spin();//循环等待回调函数
return 0;
}
加入计数器的原因：函数只检测正前方的一条射线，转弯时虽然这条射线前方没有障碍物，但小车有体积不可忽略，因此解决思路是当前方射线检测到障碍物，计数器开始工作，等待转弯5秒后再继续前行。
14. ROS 中的 IMU 惯性测量单元消息包
惯性测量单元（Inertial Measurement Unit，简称 IMU）是一种集成了多个惯性传感器的设备，用于测量物体的加速度、角速度（及有时的磁场方向），从而推算其姿态（航向、俯仰、横滚）和运动轨迹。
14.1获取IMU数据的节点
一个IMU设备可能有这三个话题：
- 加速度计输出的矢量加速度 和 陀螺仪输出的旋转角速度。
- /imu/data raw的数据 再加上 融合后的四元数姿态描述。
- 磁强计输出磁强数据。
在src工作目录下创建：
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
vscode在imu_pkg文件夹的src目录下创建imu_node.cpp
#include<ros/ros.h>
#include<sensor_msgs/Imu.h>
#include<tf/tf.h>
void IMUCallback(sensor_msgs::Imu msg)
{
if(msg.orientation_covariance[0]<0)
{
ROS_INFO("IMU数据不可用");
return;
}
tf::Quaternion quaternion(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
roll = roll * 180 / M_PI;
pitch = pitch * 180 / M_PI;
yaw = yaw * 180 / M_PI;
ROS_INFO("滚转角: %f, 俯仰角: %f, 偏航角: %f", roll, pitch, yaw);
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");//设置中文显示
ros::init(argc, argv, "imu_node");//初始化节点
ros::NodeHandle nh;//创建节点句柄
ros::Subscriber imu_sub = nh.subscribe("/imu/data", 10, IMUCallback);//订阅IMU数据 {
// 打印IMU数据
ros::spin();//循环等待回调函数
return 0;
}
Cmake里添加并编译。
add_executable(imu_node src/imu_node.cpp)
add_dependencies(imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node
${catkin_LIBRARIES}
)
打开两个终端分别输入：
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
rosrun imu_pkg imu_node
调节模拟器中的小车角度就能看到节点中接收到的角度变化
