基于RO2 Humble平台配置32线禾赛雷达，需要遵循以下步骤：### 1. 确认硬件兼容性- **RO2 Humble平台**：确认你的RO2 Humble平台是否支持32线禾赛雷达。- **禾赛雷达型号**：确保你购买的禾赛雷达型号与RO2 Humb

博客地址：https://www.cnblogs.com/zylyehuo/ 雷达型号：32线禾赛 IMU 型号：ALUBI lpms cu3 使用环境： RO2 humble 首先将雷达以及 IMU 都连接到电脑上 配置 32线禾赛雷达 查看雷达 IP 地址 安装辅助工具 sudo apt install wireshark-qt 启动辅助工具 sudo wireshark 记录下这个 IP 地址 在网页中访问雷达 IP 地址 为电脑配置 IP 地址 测试电脑能否 ping 通雷达 配置 ALUBI lpms cu3 为 IMU 的 USB 接口赋予权限 ls /dev/tty # 双击 Tab 键 sudo chmod 777 /dev/ttyUSB0 下载并编译驱动功能包 sudo apt install libpcap-dev libyaml-cpp-dev sudo apt-get update mkdir -p driver_ws/src cd driver_ws/src git clone --recurse-submodules https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0.git cd .. colcon build --symlink-install . install/local_setup.bash cd driver_ws/src git clone https://bitbucket.org/lpresearch/lpmsig1opensourcelib cd lpmsig1opensourcelib/ mkdir build cd build/ cmake .. make make package sudo dpkg -i libLpmsIG1_OpenSource-0.3.3-Linux.deb 测试 测试 32线禾赛雷达 启动 launch 文件进行测试 source install/setup.bash ros2 launch hesai_ros_driver start.py 测试 ALUBI lpms cu3 编译 linux 示例程序 cd /home/reallab/driver_ws/src/lpmsig1opensourcelib cd linux_example/ mkdir build cd build cmake .. make sudo chmod 777 /dev/ttyUSB0 ./LpmsIG1_SimpleExample /dev/ttyUSB0 921600 运行 ROS2 节点 添加 lpms_curs3_node.cpp #include <string> #include <thread> #include <chrono> #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/magnetic_field.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "sensor_msgs/msg/nav_sat_status.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" #include "std_msgs/msg/bool.hpp" #include "lpsensor/LpmsIG1I.h" #include "lpsensor/SensorDataI.h" #include "lpsensor/LpmsIG1Registers.h" struct IG1Command { short command; union Data { uint32_t i[64]; float f[64]; unsigned char c[256]; } data; int dataLength; }; class LpIG1Proxy: public rclcpp::Node { public: rclcpp::TimerBase::SharedPtr updateTimer; // Publisher rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub; rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr mag_pub; rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr autocalibration_status_pub; // Service rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr autocalibration_serv; rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr autoReconnect_serv; rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr gyrocalibration_serv; rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resetHeading_serv; rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr getImuData_serv; rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr setStreamingMode_serv; rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr setCommandMode_serv; sensor_msgs::msg::Imu imu_msg; sensor_msgs::msg::MagneticField mag_msg; // Parameters std::string comportNo; int baudrate; bool autoReconnect; std::string frame_id; int rate; LpIG1Proxy() : Node("lpms_ig1_node") { // Get node parameters this->declare_parameter<std::string>("port", "/dev/ttyUSB0"); this->declare_parameter<int>("baudrate", 921600); this->declare_parameter<bool>("autoreconnect", true); this->declare_parameter<std::string>("frame_id", "imu"); this->declare_parameter<int>("rate", 200); this->get_parameter("port", comportNo); this->get_parameter("baudrate", baudrate); this->get_parameter("autoreconnect", autoReconnect); this->get_parameter("frame_id", frame_id); this->get_parameter("rate", rate); // Create LpmsIG1 object sensor1 = IG1Factory(); sensor1->setVerbose(VERBOSE_INFO); sensor1->setAutoReconnectStatus(autoReconnect); RCLCPP_INFO(this->get_logger(), "Settings"); RCLCPP_INFO(this->get_logger(), "Port: %s", comportNo.c_str()); RCLCPP_INFO(this->get_logger(), "Baudrate: %d", baudrate); RCLCPP_INFO(this->get_logger(), "Auto reconnect: %s", autoReconnect? "Enabled":"Disabled"); imu_pub = this->create_publisher<sensor_msgs::msg::Imu>("data",1); mag_pub = this->create_publisher<sensor_msgs::msg::MagneticField>("mag",1); autocalibration_status_pub = this->create_publisher<std_msgs::msg::Bool>("is_autocalibration_active",1); autocalibration_serv = this->create_service<std_srvs::srv::SetBool>( "enable_gyro_autocalibration",std::bind(&LpIG1Proxy::setAutocalibration, this,std::placeholders::_1, std::placeholders::_2)); autoReconnect_serv = this->create_service<std_srvs::srv::SetBool>( "enable_auto_reconnect",std::bind(&LpIG1Proxy::setAutoReconnect, this, std::placeholders::_1, std::placeholders::_2)); gyrocalibration_serv = this->create_service<std_srvs::srv::Trigger>( "calibrate_gyroscope",std::bind(&LpIG1Proxy::calibrateGyroscope, this, std::placeholders::_1, std::placeholders::_2)); resetHeading_serv = this->create_service<std_srvs::srv::Trigger>( "reset_heading",std::bind(&LpIG1Proxy::resetHeading, this, std::placeholders::_1, std::placeholders::_2)); getImuData_serv = this->create_service<std_srvs::srv::Trigger>( "get_imu_data",std::bind(&LpIG1Proxy::getImuData, this, std::placeholders::_1, std::placeholders::_2)); setStreamingMode_serv = this->create_service<std_srvs::srv::Trigger>( "set_streaming_mode",std::bind(&LpIG1Proxy::setStreamingMode, this, std::placeholders::_1, std::placeholders::_2)); setCommandMode_serv = this->create_service<std_srvs::srv::Trigger>( "set_command_mode",std::bind(&LpIG1Proxy::setCommandMode, this, std::placeholders::_1, std::placeholders::_2)); // Connects to sensor if (!sensor1->connect(comportNo, baudrate)) { RCLCPP_ERROR(this->get_logger(), "Error connecting to sensor\n"); sensor1->release(); std::this_thread::sleep_for(std::chrono::milliseconds(3000)); } do { RCLCPP_INFO(this->get_logger(), "Waiting for sensor to connect %d", sensor1->getStatus()); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } while( rclcpp::ok() && ( !(sensor1->getStatus() == STATUS_CONNECTED) && !(sensor1->getStatus() == STATUS_CONNECTION_ERROR) ) ); if (sensor1->getStatus() == STATUS_CONNECTED) { RCLCPP_INFO(this->get_logger(), "Sensor connected"); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); sensor1->commandGotoStreamingMode(); } else { RCLCPP_INFO(this->get_logger(), "Sensor connection error: %d.", sensor1->getStatus()); rclcpp::shutdown(); } } ~LpIG1Proxy(void) { sensor1->release(); } void update() { static bool runOnce = false; if (sensor1->getStatus() == STATUS_CONNECTED && sensor1->hasImuData()) { if (!runOnce) { publishIsAutocalibrationActive(); runOnce = true; } IG1ImuDataI sd; sensor1->getImuData(sd); /* Fill the IMU message */ // Fill the header imu_msg.header.stamp = this->now(); imu_msg.header.frame_id = frame_id; // Fill orientation quaternion imu_msg.orientation.w = sd.quaternion.data[0]; imu_msg.orientation.x = sd.quaternion.data[1]; imu_msg.orientation.y = sd.quaternion.data[2]; imu_msg.orientation.z = sd.quaternion.data[3]; // Fill angular velocity data // - scale from deg/s to rad/s imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180; imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180; imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180; // Fill linear acceleration data imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81; imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81; imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81; /* Fill the magnetometer message */ mag_msg.header.stamp = imu_msg.header.stamp; mag_msg.header.frame_id = frame_id; // Units are microTesla in the LPMS library, Tesla in ROS. mag_msg.magnetic_field.x = sd.magRaw.data[0]*1e-6; mag_msg.magnetic_field.y = sd.magRaw.data[1]*1e-6; mag_msg.magnetic_field.z = sd.magRaw.data[2]*1e-6; // Publish the messages imu_pub->publish(imu_msg); mag_pub->publish(mag_msg); } } void run(void) { updateTimer = this->create_wall_timer( std::chrono::milliseconds(1000 / abs(rate)), std::bind(&LpIG1Proxy::update, this)); } void publishIsAutocalibrationActive() { std_msgs::msg::Bool msg; IG1SettingsI settings; sensor1->getSettings(settings); msg.data = settings.enableGyroAutocalibration; autocalibration_status_pub->publish(msg); } /////////////////////////////////////////////////// // Service Callbacks /////////////////////////////////////////////////// bool setAutocalibration ( const std::shared_ptr<std_srvs::srv::SetBool::Request> req, std::shared_ptr<std_srvs::srv::SetBool::Response> res) { RCLCPP_INFO(this->get_logger(), "set_autocalibration"); // clear current settings IG1SettingsI settings; sensor1->getSettings(settings); // Send command cmdSetEnableAutocalibration(req->data); std::this_thread::sleep_for(std::chrono::milliseconds(200)); cmdGetEnableAutocalibration(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); double retryElapsedTime = 0; int retryCount = 0; while (!sensor1->hasSettings()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); RCLCPP_INFO(this->get_logger(), "set_autocalibration wait"); retryElapsedTime += 0.1; if (retryElapsedTime > 2.0) { retryElapsedTime = 0; cmdGetEnableAutocalibration(); retryCount++; } if (retryCount > 5) break; } RCLCPP_INFO(this->get_logger(), "set_autocalibration done"); // Get settings sensor1->getSettings(settings); std::string msg; if (settings.enableGyroAutocalibration == req->data) { res->success = true; msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False")); } else { res->success = false; msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False")); } RCLCPP_INFO(this->get_logger(), "%s", msg.c_str()); res->message = msg; publishIsAutocalibrationActive(); return res->success; } // Auto reconnect bool setAutoReconnect ( const std::shared_ptr<std_srvs::srv::SetBool::Request> req, std::shared_ptr<std_srvs::srv::SetBool::Response> res) { RCLCPP_INFO(this->get_logger(), "set_auto_reconnect"); sensor1->setAutoReconnectStatus(req->data); res->success = true; std::string msg; msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False")); RCLCPP_INFO(this->get_logger(), "%s", msg.c_str()); res->message = msg; return res->success; } // reset heading bool resetHeading ( const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) { RCLCPP_INFO(this->get_logger(), "reset_heading"); // Send command cmdResetHeading(); res->success = true; res->message = "[Success] Heading reset"; return true; } bool calibrateGyroscope ( const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) { RCLCPP_INFO(this->get_logger(), "calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds"); cmdCalibrateGyroscope(); std::this_thread::sleep_for(std::chrono::milliseconds(4000)); res->success = true; res->message = "[Success] Gyroscope calibration procedure completed"; RCLCPP_INFO(this->get_logger(), "calibrate_gyroscope: Gyroscope calibration procedure completed"); return true; } bool getImuData ( const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) { cmdGotoCommandMode(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); cmdGetImuData(); res->success = true; res->message = "[Success] Get imu data"; return true; } bool setStreamingMode ( const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) { cmdGotoStreamingMode(); res->success = true; res->message = "[Success] Set streaming mode"; return true; } bool setCommandMode ( const std::shared_ptr<std_srvs::srv::Trigger::Request> req, std::shared_ptr<std_srvs::srv::Trigger::Response> res) { cmdGotoCommandMode(); res->success = true; res->message = "[Success] Set command mode"; return true; } /////////////////////////////////////////////////// // Helpers /////////////////////////////////////////////////// void cmdGotoCommandMode () { IG1Command cmd; cmd.command = GOTO_COMMAND_MODE; cmd.dataLength = 0; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdGotoStreamingMode () { IG1Command cmd; cmd.command = GOTO_STREAM_MODE; cmd.dataLength = 0; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdGetImuData() { IG1Command cmd; cmd.command = GET_IMU_DATA; cmd.dataLength = 0; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdCalibrateGyroscope() { IG1Command cmd; cmd.command = START_GYR_CALIBRATION; cmd.dataLength = 0; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdResetHeading() { IG1Command cmd; cmd.command = SET_ORIENTATION_OFFSET; cmd.dataLength = 4; cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdSetEnableAutocalibration(int status) { IG1Command cmd; cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION; cmd.dataLength = 4; cmd.data.i[0] = status; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } void cmdGetEnableAutocalibration() { IG1Command cmd; cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION; cmd.dataLength = 0; sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c); } private: // Access to LPMS data IG1I* sensor1; }; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto lpIG1 = std::make_shared<LpIG1Proxy>(); lpIG1->run(); rclcpp::spin(lpIG1); rclcpp::shutdown(); return 0; } 修改 CMakeLists.txt cmake_minimum_required(VERSION 3.8) project(lpms_ig1) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) add_executable(lpms_ig1_node src/lpms_ig1_node.cpp ) add_executable(lpms_be1_node src/lpms_be1_node.cpp ) add_executable(lpms_nav3_node src/lpms_nav3_node.cpp ) add_executable(lpms_ig1_rs485_node src/lpms_ig1_rs485_node.cpp ) add_executable(lpms_ig1_rs485_client src/lpms_ig1_rs485_client.cpp ) add_executable(lpms_si1_node src/lpms_si1_node.cpp ) add_executable(quat_to_euler_node src/quat_to_euler_node.cpp ) add_executable(imudata_rad_to_deg_node src/imudata_rad_to_deg_node.cpp ) # 新增的 lpms_curs3_node add_executable(lpms_curs3_node src/lpms_curs3_node.cpp ) target_link_libraries(lpms_ig1_node LpmsIG1_OpenSourceLib) target_link_libraries(lpms_be1_node LpmsIG1_OpenSourceLib) target_link_libraries(lpms_nav3_node LpmsIG1_OpenSourceLib) target_link_libraries(lpms_ig1_rs485_node LpmsIG1_OpenSourceLib) target_link_libraries(lpms_si1_node LpmsIG1_OpenSourceLib) # 新增的 lpms_curs3_node 链接库 target_link_libraries(lpms_curs3_node LpmsIG1_OpenSourceLib) ament_target_dependencies(lpms_ig1_node rclcpp std_msgs std_srvs sensor_msgs) ament_target_dependencies(lpms_be1_node rclcpp std_msgs std_srvs sensor_msgs) ament_target_dependencies(lpms_nav3_node rclcpp std_msgs std_srvs sensor_msgs) ament_target_dependencies(lpms_ig1_rs485_node rclcpp std_msgs std_srvs sensor_msgs) ament_target_dependencies(lpms_ig1_rs485_client rclcpp std_srvs) ament_target_dependencies(lpms_si1_node rclcpp std_msgs std_srvs sensor_msgs) ament_target_dependencies(quat_to_euler_node rclcpp sensor_msgs tf2) ament_target_dependencies(imudata_rad_to_deg_node rclcpp sensor_msgs tf2) # 新增的 lpms_curs3_node 依赖 ament_target_dependencies(lpms_curs3_node rclcpp std_msgs std_srvs sensor_msgs) install(TARGETS lpms_ig1_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS lpms_be1_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS lpms_nav3_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS lpms_ig1_rs485_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS lpms_ig1_rs485_client DESTINATION lib/${PROJECT_NAME}) install(TARGETS lpms_si1_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS quat_to_euler_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS imudata_rad_to_deg_node DESTINATION lib/${PROJECT_NAME}) # 新增的 lpms_curs3_node 安装 install(TARGETS lpms_curs3_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # uncomment the line when a copyright and license is not present in all source files #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() ament_package() 运行指令 cd /home/reallab/driver_ws ln -s ./src/lpmsig1opensourcelib/ros2_example ./lpms_ig1_ros2_example source /opt/ros/humble/setup.bash colcon build sudo chmod 777 /dev/ttyUSB0 source install/setup.bash ros2 run lpms_ig1 lpms_curs3_node --ros-args --param port:=/dev/ttyUSB0 --param baudrate:=921600 rviz2 编译 ROS2 示例程序 cd /home/reallab/driver_ws ln -s ./src/lpmsig1opensourcelib/ros2_example ./lpms_ig1_ros2_example source /opt/ros/humble/setup.bash colcon build sudo chmod 777 /dev/ttyUSB0 source install/setup.bash ros2 launch lpms_ig1 lpms_si1_launch.py 在 rviz 中可视化 sudo apt install ros-humble-rviz-imu-plugin rviz2