Create ROS2 launch package for subscribing/publishing LiDAR topic

0. Package Structure

laser_subscriber/
├── launch/
│ └── laser_subscriber_launch.py
├── config/
│ └── rviz_laser_subscriber.rviz
│ └── params.yaml
├── src/
│ └── laser_subscriber.cpp
├── CMakeLists.txt
├── package.xml

1. Create a package

$ cd ~/robot_ws/src/
$ ros2 pkg create --build-type ament_cmake laser_subscriber --dependencies rclcpp sensor_msgs pcl_conversions pcl_msgs

2. C++ source code (src/laser_subscriber.cpp)

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_laser;
double voxel_leaf_size = 0.1;

void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr laser)
{
    RCLCPP_INFO(
        rclcpp::get_logger("laser_subscribe_node"),
        "Subscribed laser! Voxel leaf size = %.2f",
        voxel_leaf_size
    );

    // ROS → PCL
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*laser, *input_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::VoxelGrid<pcl::PointXYZ> voxel;
    voxel.setInputCloud(input_cloud);
    voxel.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
    voxel.filter(*output_cloud);

    // PCL → ROS
    sensor_msgs::msg::PointCloud2 output;
    pcl::toROSMsg(*output_cloud, output);
    output.header = laser->header;

    pub_laser->publish(output);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("laser_subscribe_node");

    // Declare parameters with defaults
    node->declare_parameter<double>("voxel_leaf_size", 5.0);

    // Get params
    node->get_parameter("voxel_leaf_size", voxel_leaf_size);

    // Publisher 
    pub_laser = node->create_publisher<sensor_msgs::msg::PointCloud2>("/ouster/voxel", 10);

    // Subscriber
    auto sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
        "/ouster/points", 10, cloud_callback);

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

3. CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(laser_subscriber)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_msgs REQUIRED)
find_package(PCL REQUIRED COMPONENTS common filters io)

include_directories(${PCL_INCLUDE_DIRS})

add_executable(laser_subscriber_node src/laser_subscriber.cpp)

ament_target_dependencies(laser_subscriber_node
  rclcpp
  sensor_msgs
  pcl_conversions
  pcl_msgs
)

target_link_libraries(laser_subscriber_node
  ${PCL_COMMON_LIBRARIES}
  ${PCL_FILTERS_LIBRARIES}
  ${PCL_IO_LIBRARIES}
)

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

install(DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME}/
)

ament_package()

4. rviz configuration

locate ‘rviz_laser_subscriber.rviz’ in ‘config’ folder

5. params.yaml file

laser_subscriber:
  ros__parameters:
    voxel_leaf_size: 0.5

6. launch file

from launch import LaunchDescription
from launch_ros.actions import Node
import os


def generate_launch_description():

    pkg_name = 'laser_subscriber'
    pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && \
        colcon_cd %s && pwd"' % pkg_name).read().strip()
    
    return LaunchDescription([
        Node(
            package='laser_subscriber',
            executable='laser_subscriber_node',
            name='laser_subscriber',
            output='screen',
            parameters= [os.path.join(pkg_dir, 'config', 'params.yaml')]
        ),

        Node(
            package='rviz2',
            namespace='',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', [os.path.join(pkg_dir, 'config', 'rviz_laser_subscriber.rviz')]]
        )
    ])

7. Run

$ colcon build --packages-select laser_subscriber
$ source install/setup.bash
$ ros2 launch laser_subscriber laser_subscriber_launch.py

8. Play ROS Bag File

$ ros2 bag play {bagfile}

9. Results

Read voxel filter parameters from ‘config/params.yaml’ and publish laser topic

Post Views: 11