从Rviz观测雷达数据到节点节点激光雷达避障

---

需要现在git上获取开源项目
在src目录下克隆软件包

git clone https://github.com/6-robot/wpr_simulation.git

一、激光雷达是什么？

按照测量维度可以分为单线雷达和多线雷达
按照测量原理可以分为三角测距雷达和TOF雷达
按照工作方式可以分为机械旋转雷达和固态雷达

二、使用Rviz观测传感器数据

Rviz是可视化工具，方便对机器人状态实时观测的辅助工具

roslaunch wpr_simulation wpb_simple.launch

rviz

fixed frame选择base_footprint，然后点击左侧add添加机器人模型
添加激光雷达显示条
修改激光雷达的话题名称和显示点大小
rviz仿真工具与gazebo可视化工具的关系

保存可视化工具的配置
手动：File->save config as->在主目录下保存lidar.rivz，下次使用要手动点open config
自动：在launch中加载rviz配置文件

roslaunch wpr_simulation wpr_rviz.launch

三、激光雷达消息包数据格式

传感器数据包：sensor_msgs，找到消息类型：LaserScan

在运行仿真和可视化工具的前提下，显示scan话题里面的消息。

rostopic echo /scan --noarr #把数组折叠起来

四、订阅者节点的应用：获取激光雷达数据

编写获取激光雷达数据的案例

进入catkin_ws/src目录：

catkin_create_pkg lidar_pkg rospy roscpp sensor_msgs

进入src目录创建lidar_node.cpp

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

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    
    int nMid = nNum/2;//180°为正前方
    float fMidDist = msg.ranges[nMid];
    ROS_INFO("前方测距 ranges[%d] = %f 米", nMid, fMidDist); 
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");//设置中文编码
    ros::init(argc,argv,"demo_lidar_data");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);

    ros::spin();//保持节点运行不退出
}

修改CMakeLists.txt

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

编译运行：

roslaunch wpr_simulation wpb_simple.launch
rosrun lidar_pkg lidar_node

五、激光雷达避障

#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();
}