ROS2中的激光雷达

ROS2中的激光雷达的节点会在指定名称的话题里发布消息，话题名称在REP-138中推荐为/scan

激光雷达的消息类型为sensor_msgs::LaserScan

RViz2查看激光雷达数据

我们先打开机器人仿真环境，机器人的脚底就是激光雷达

cd ros2_ws
source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple_launch.py

打开新窗口加载环境变量后打开rviz2

source ~/ros2_ws/install/setup.bash
rviz2

打开之后按照书本设置好，可以看到点云和机器人在rviz2中显示

设置好之后保存rviz2设置以便以后直接加载，加载命令：

rivz2 -d lidar.rviz

激光雷达数据获取

#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]=%fm",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;
}

find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)

add_executable(lidar_data src/lidar_data.cpp)
ament_target_dependencies(lidar_data "rclcpp" "sensor_msgs")

install(
  TARGETS lidar_data
  DESTINATION lib/${PROJECT_NAME}
)

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>

打开机器人仿真程序和我们的数据获取程序，结果如下，读取成功

基于激光雷达的避障实现

实现一个避障程序，程序订阅激光雷达的“/scan“话题，从中接受激光雷达发来的sensor_mgs::LaserScan类型消息报，并解析出雷达测距值，根据测距值让机器人做出反应，前方没有障碍物就前进，前方遇到障碍物就转向，将根据测距值计算出来的速度值打包成geometry_msgs::Twist类型的消息包，发布到cmd_vel话题中

#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]=%fm",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;
}

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}
)

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>