集成第三方SDK（动态库 .so 和头文件）到ROS23D雷达功能包的流程

由于很多时候拿到的硬件没有适配ros2的功能包，一般只有基础cdk，就需要我们自己构建功能包。

以下是构建3d激光雷达功能包记录

以HPS3D160面阵雷达为例

接受雷达数据发布点晕

一、功能包目录结构示例

lidar_3d_driver/                # 功能包根目录
├── include/                    # 头文件目录
│   └── lidar_3d_driver/        # 包含包内头文件的子目录（命名空间）
│       ├── lidar_driver.hpp    # 你的驱动头文件
│       └── sdk_header.h        # SDK头文件（可软链或复制）
├── lib/                        # 第三方动态库目录
│   └── libHPS3DSDK64.so        # 第三方SDK动态库
├── src/                        # 源代码目录
│   └── lidar_node.cpp          # 节点实现代码
├── CMakeLists.txt              # 构建配置文件
└── package.xml                 # 包描述文件

二、功能包构建思路

步骤	说明
1. 头文件管理	把自己写的头文件放include/lidar_3d_driver/，SDK头文件放include/或软链
2. 动态库管理	把第三方.so库放lib/目录，方便统一管理
3. CMake配置	- 添加include_directories包含头文件路径<br>- 添加link_directories包含库路径<br>- 在target_link_libraries链接SDK库<br>- 设置rpath保证运行时能找到库
4. package.xml依赖	声明ROS2依赖包，方便自动安装和查找
5. 编写节点代码	包含SDK头文件，调用SDK接口，实现数据采集和ROS消息发布
6. 编译和运行	使用colcon build编译，运行时设置LD_LIBRARY_PATH或依赖rpath

 

三、具体操作

1、假设包名为lidar_3d_driver，并指定依赖：

ros2 pkg create --build-type ament_cmake lidar_3d_driver --dependencies rclcpp sensor_msgs pcl_conversions pcl_ros tf2_ros geometry_msgs

2、进入功能包下创建必要目录：

mkdir -p include/lidar_3d_driver
mkdir lib
mkdir src

3、把SDK的.so文件复制到lib/目录

要选择对应的.so

4、创建头文件目录

mkdir -p ~/ros2_ws/src/lidar_3d_driver/include/lidar_3d_driver

 如果你有自己写的头文件，比如 lidar_driver.hpp

或者第三方SDK的头文件都放到里面并确保 CMakeLists.txt 中包含头文件路径

include_directories(
  include
  ${CMAKE_CURRENT_SOURCE_DIR}/include
)

5、编写节点代码：

具体根据提供的sdk函数调用写

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "lidar_3d_driver/HPS3DSDK.h"  // SDK头文件路径根据实际调整

class Lidar3DNode : public rclcpp::Node
{
public:
  Lidar3DNode() : Node("lidar_3d_node")
  {
    pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("points_raw", 10);
    // TODO: 初始化SDK，采集点云并发布
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_pub_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Lidar3DNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

6、修改 CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(lidar_3d_driver)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)

include_directories(
  include
  ${CMAKE_CURRENT_SOURCE_DIR}/include
)

link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib)

add_executable(lidar_3d_node src/lidar_node.cpp)

ament_target_dependencies(lidar_3d_node
  rclcpp
  sensor_msgs
  pcl_conversions
  pcl_ros
  tf2_ros
  geometry_msgs
)

target_link_libraries(lidar_3d_node
  HPS3DSDK64  # 这里写你的SDK库名，去掉lib和.so
)

set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_INSTALL_RPATH "${CMAKE_CURRENT_SOURCE_DIR}/lib")

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

ament_package()

7、修改 package.xml

<package format="3">
  <name>lidar_3d_driver</name>
  <version>0.0.1</version>
  <description>3D LiDAR driver with SDK integration</description>
  <maintainer email="your_email@example.com">Your Name</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>pcl_conversions</depend>
  <depend>pcl_ros</depend>
  <depend>tf2_ros</depend>
  <depend>geometry_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

四、问题解决

1、找不到iso文件

dayang@dayang-virtual-machine:~/ros2_ws$ ros2 run hps3d_driver hps3d_node 
/home/dayang/ros2_ws/install/hps3d_driver/lib/hps3d_driver/hps3d_node: error while loading shared libraries: libHPS3DSDK64_1-8-5.so: cannot open shared object file: No such file or directory
[ros2run]: Process exited with failure 127

这个是因为系统找不到你的iso文件

需要更新环境

export LD_LIBRARY_PATH=~/ros2_ws/src/lidar_3d_driver/lib:$LD_LIBRARY_PATH
ros2 run lidar_3d_driver lidar_3d_node

根据自己目录修改

2、rviz里面接收点云数据full

reason 'discarding message because the queue is full'
[INFO] [1745650955.306614335] [rviz]: Message Filter dropping message: frame 'hps' at time 1745650954.620 for reason 'discarding message because the queue is full'
[INFO] [1745650955.370374514] [rviz]: Message Filter dropping message: frame 'hps' at time 1745650954.685 for reason 'discarding message because the queue is full'
[INFO] [1745650955.435076656] [rviz]: Message Filter dropping message: frame 'hps' at time 1745650954.753 for reason 'discarding message because the queue is full'
[INFO] [1745650955.499252808] [rviz]: Message Filter dropping message: frame 'hps' at time 1745650954.820 for reason 'discarding message because the queue is full'
^C[INFO] [1745650955.555744098] [rclcpp]: signal_handler(signum=2)

这是因为没有点云数据和map数据的tf转化，导致rviz放弃数据

解决方法使用静态发布节点发布map到hps的tf

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

class StaticTfPublisher : public rclcpp::Node
{
public:
  StaticTfPublisher() : Node("static_tf_publisher")
  {
    static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    geometry_msgs::msg::TransformStamped static_transformStamped;
    static_transformStamped.header.stamp = this->now();
    static_transformStamped.header.frame_id = "map";
    static_transformStamped.child_frame_id = "hps";
    static_transformStamped.transform.translation.x = 0.0;
    static_transformStamped.transform.translation.y = 0.0;
    static_transformStamped.transform.translation.z = 0.0;
    static_transformStamped.transform.rotation.x = 0.0;
    static_transformStamped.transform.rotation.y = 0.0;
    static_transformStamped.transform.rotation.z = 0.0;
    static_transformStamped.transform.rotation.w = 1.0;

    static_broadcaster_->sendTransform(static_transformStamped);

    RCLCPP_INFO(this->get_logger(), "Static transform from 'map' to 'hps' published");
  }

private:
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StaticTfPublisher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

成功解决问题;

五、代码参考

 

#include "hps3d_driver/hps3d_node.hpp"
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <stdexcept>
#include <sstream>
#include <iomanip>
#include <signal.h>
#include <chrono>
#include <thread>

HPS3DNode::HPS3DNode() : Node("hps3d_node"), device_handle_(-1)
{
  pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("hps_output", 10);

  if (HPS3D_MeasureDataInit(&measure_data_) != HPS3D_RET_OK) {
    RCLCPP_ERROR(this->get_logger(), "Failed to initialize measure data");
    throw std::runtime_error("MeasureDataInit failed");
  }

  connectDevice();

  static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
// 发布 map -> map 的静态变换，保证 map 坐标系存在
geometry_msgs::msg::TransformStamped map_transform;
map_transform.header.stamp = this->now();
map_transform.header.frame_id = "map";
map_transform.child_frame_id = "map";
map_transform.transform.translation.x = 0.0;
map_transform.transform.translation.y = 0.0;
map_transform.transform.translation.z = 0.0;
map_transform.transform.rotation.x = 0.0;
map_transform.transform.rotation.y = 0.0;
map_transform.transform.rotation.z = 0.0;
map_transform.transform.rotation.w = 1.0;

static_broadcaster_->sendTransform(map_transform);

// 你已有的 map -> hps 静态变换
geometry_msgs::msg::TransformStamped static_transformStamped;
static_transformStamped.header.stamp = this->now();
static_transformStamped.header.frame_id = "map";  
static_transformStamped.child_frame_id = "hps";
static_transformStamped.transform.translation.x = 0.0;
static_transformStamped.transform.translation.y = 0.0;
static_transformStamped.transform.translation.z = 0.0;
static_transformStamped.transform.rotation.x = 0.0;
static_transformStamped.transform.rotation.y = 0.0;
static_transformStamped.transform.rotation.z = 0.0;
static_transformStamped.transform.rotation.w = 1.0;

static_broadcaster_->sendTransform(static_transformStamped);


  // 注册事件回调
  HPS3D_RegisterEventCallback(&HPS3DNode::eventCallbackStatic, this);

  // 启动采集
  int ret = HPS3D_StartCapture(device_handle_);
  if (ret != HPS3D_RET_OK) {
    RCLCPP_ERROR(this->get_logger(), "StartCapture failed with code %d", ret);
    throw std::runtime_error("StartCapture failed");
  }

  RCLCPP_INFO(this->get_logger(), "Device connected and capturing");

  // 注册信号处理，优雅退出
  signal(SIGINT, [](int) {
    RCLCPP_INFO(rclcpp::get_logger("hps3d_node"), "SIGINT received, shutting down...");
    rclcpp::shutdown();
  });
}

HPS3DNode::~HPS3DNode()
{
  onShutdown();
}

void HPS3DNode::connectDevice()
{
  std::string ip = "192.168.0.10";  // 修改为你的设备IP
  uint16_t port = 12345;             // 修改为你的设备端口

  int ret = HPS3D_EthernetConnectDevice(const_cast<char*>(ip.c_str()), port, &device_handle_);
  if (ret != HPS3D_RET_OK) {
    RCLCPP_ERROR(this->get_logger(), "Failed to connect device, ret=%d", ret);
    throw std::runtime_error("Device connect failed");
  }
  RCLCPP_INFO(this->get_logger(), "Device connected, handle=%d", device_handle_);
}

void HPS3DNode::disconnectDevice()
{
  if (device_handle_ >= 0) {
    HPS3D_CloseDevice(device_handle_);
    device_handle_ = -1;
    RCLCPP_INFO(this->get_logger(), "Device disconnected");
  }
}

void HPS3DNode::onShutdown()
{
  HPS3D_StopCapture(device_handle_);
  HPS3D_UnregisterEventCallback();
  HPS3D_MeasureDataFree(&measure_data_);
  disconnectDevice();
}

void HPS3DNode::eventCallbackStatic(int handle, int eventType, uint8_t* data, int dataLen, void* userPara)
{
  auto node = static_cast<HPS3DNode*>(userPara);
  node->eventCallback(handle, eventType, data, dataLen);
}

void HPS3DNode::eventCallback(int handle, int eventType, uint8_t* data, int dataLen)
{
  static int count = 0;
  count++;
  if (count % 3 != 0)  // 每3帧发布一次
    return;

  (void)handle;
  (void)dataLen;

  switch ((HPS3D_EventType_t)eventType)
  {
    case HPS3D_SIMPLE_ROI_EVEN:
    case HPS3D_FULL_ROI_EVEN:
    case HPS3D_FULL_DEPTH_EVEN:
    case HPS3D_SIMPLE_DEPTH_EVEN:
    {
      if (HPS3D_ConvertToMeasureData(data, &measure_data_, (HPS3D_EventType_t)eventType) <= 0) {
        RCLCPP_WARN(this->get_logger(), "Failed to convert measure data");
        return;
      }
      sensor_msgs::msg::PointCloud2 pc_msg;
      convertToPointCloud2(measure_data_, pc_msg);
      pc_pub_->publish(pc_msg);
      break;
    }
    case HPS3D_SYS_EXCEPTION_EVEN:
    {
      RCLCPP_ERROR(this->get_logger(), "System exception event received");
      break;
    }
    case HPS3D_DISCONNECT_EVEN:
    {
      RCLCPP_WARN(this->get_logger(), "Device disconnected");
      disconnectDevice();
      break;
    }
    case HPS3D_NULL_EVEN:
    default:
      // Ignore other events
      break;
  }
}


void HPS3DNode::convertToPointCloud2(const HPS3D_MeasureData_t& measure_data, sensor_msgs::msg::PointCloud2& pc_msg)
{
  const auto& cloud = measure_data.full_depth_data.point_cloud_data;

  pc_msg.header.stamp = this->now();
  pc_msg.header.frame_id = "hps";

  pc_msg.height = cloud.height;
  pc_msg.width = cloud.width;
  pc_msg.is_dense = false;
  pc_msg.is_bigendian = false;

  sensor_msgs::PointCloud2Modifier modifier(pc_msg);
  modifier.setPointCloud2FieldsByString(1, "xyz");
  modifier.resize(pc_msg.width * pc_msg.height);

  sensor_msgs::PointCloud2Iterator<float> iter_x(pc_msg, "x");
  sensor_msgs::PointCloud2Iterator<float> iter_y(pc_msg, "y");
  sensor_msgs::PointCloud2Iterator<float> iter_z(pc_msg, "z");

  for (size_t i = 0; i < cloud.points; ++i, ++iter_x, ++iter_y, ++iter_z) {
    *iter_x = cloud.point_data[i].x / 1000.0f;  // mm to meters
    *iter_y = cloud.point_data[i].y / 1000.0f;
    *iter_z = cloud.point_data[i].z / 1000.0f;
  }
}