使用G1的建好圖後,,點雲地圖是保存在PC1上的,,用戶可以通過訂閱話題的方式獲取地圖數據。。。。全局地圖點雲數據話題:rt/unitree/slam_relocations/global_map,,,,該話題數據僅在開始定位後發送一次。。。。
DDS數據類型:sensor_msgs::msg::dds_::PointCloud2_
ROS2話題數據類型:sensor_msgs::msg::PointCloud2
方法1:C++代碼(ROS2話題方式)
主程序代碼:
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <filesystem>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <memory>
#define TOPIC "/unitree/slam_relocations/global_map" // G1全局地圖話題
class GlobalMapSaver : public rclcpp::Node
{
public:
GlobalMapSaver() : Node("global_map_saver"), received_(false)
{
// 創建保存目錄
save_dir_ = "./global_maps";
if (!std::filesystem::exists(save_dir_)) {
std::filesystem::create_directories(save_dir_);
}
// 創建訂閱者
subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
TOPIC,
10,
std::bind(&GlobalMapSaver::map_callback, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "等待接收全局地圖數據...");
}
private:
void map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
if (received_) {
return;
}
received_ = true;
RCLCPP_INFO(this->get_logger(), "接收到全局地圖數據,,,,開始處理...");
RCLCPP_INFO(this->get_logger(), "數據大小: %zu 字節", msg->data.size());
try {
// 將ROS PointCloud2轉換為PCL點雲
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
RCLCPP_INFO(this->get_logger(), "提取到 %zu 個點", cloud->points.size());
if (cloud->points.empty()) {
RCLCPP_WARN(this->get_logger(), "點雲數據為空,,,,無法保存");
return;
}
// 移除NaN點
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
RCLCPP_INFO(this->get_logger(), "移除NaN後剩餘 %zu 個有效點", cloud->points.size());
if (cloud->points.empty()) {
RCLCPP_ERROR(this->get_logger(), "點雲數據全部為NaN,,,,無法保存");
return;
}
// 生成時間戳文件名
auto now = std::chrono::system_clock::now();
auto time_t_now = std::chrono::system_clock::to_time_t(now);
std::stringstream ss;
ss << std::put_time(std::localtime(&time_t_now), "%Y%m%d_%H%M%S");
std::string timestamp = ss.str();
// 保存為PCD文件
std::string pcd_path = save_dir_ + "/global_map_" + timestamp + ".pcd";
if (pcl::io::savePCDFileBinary(pcd_path, *cloud) == 0) {
RCLCPP_INFO(this->get_logger(), "PCD文件保存成功: %s", pcd_path.c_str());
} else {
RCLCPP_ERROR(this->get_logger(), "PCD文件保存失敗: %s", pcd_path.c_str());
}
// 保存為PLY文件
std::string ply_path = save_dir_ + "/global_map_" + timestamp + ".ply";
if (pcl::io::savePLYFileBinary(ply_path, *cloud) == 0) {
RCLCPP_INFO(this->get_logger(), "PLY文件保存成功: %s", ply_path.c_str());
} else {
RCLCPP_ERROR(this->get_logger(), "PLY文件保存失敗: %s", ply_path.c_str());
}
// 計算點雲範圍
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(*cloud, min_pt, max_pt);
RCLCPP_INFO(this->get_logger(), "點雲數量: %zu 個點", cloud->points.size());
RCLCPP_INFO(this->get_logger(), "點雲範圍: X[%.2f, %.2f], Y[%.2f, %.2f], Z[%.2f, %.2f]",
min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z);
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "處理點雲數據時出錯: %s", e.what());
}
RCLCPP_INFO(this->get_logger(), "處理完成");
rclcpp::shutdown();
}
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
std::string save_dir_;
bool received_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<GlobalMapSaver>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
CMakeLists.txt 配置
cmake_minimum_required(VERSION 3.8)
project(global_map_saver)
# 設置C++標準
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# 查找依賴包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(PCL 1.12 REQUIRED COMPONENTS common io)
# 包含目錄
include_directories(
${PCL_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)
# 添加可執行文件
add_executable(global_map_saver src/global_map_saver.cpp)
# 鏈接庫
target_link_libraries(global_map_saver
${rclcpp_LIBRARIES}
${sensor_msgs_LIBRARIES}
${PCL_LIBRARIES}
)
# 安裝目標
install(TARGETS global_map_saver
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
package.xml 配置
<?xml version="1.0"?>
<?xml-model href="https://download.ros.org/schema/package_format3.xsd" schematypens="https://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>global_map_saver</name>
<version>1.0.0</version>
<description>Global Map Saver Node for LIO-SAM</description>
<maintainer email="your-email@example.com">Your Name</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_msgs</depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<exec_depend>pcl_conversions</exec_depend>
<exec_depend>libpcl-all</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
編譯
1.創建ROS2工作空間(如果還沒有):
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
2.創建package並添加代碼:
ros2 pkg create --build-type ament_cmake global_map_saver
# 將上面的C++代碼保存為 src/global_map_saver.cpp
# 將CMakeLists.txt和package.xml替換為上面的內容
3.安裝PCL庫:
sudo apt-get install libpcl-dev
4.編譯:
cd ~/ros2_ws
colcon build --packages-select global_map_saver
source install/setup.bash
使用方法
1.運行保存腳本:
```bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 run global_map_saver global_map_saver
```
2.開啟定位,,,,使用SLAM導航服務接口開啟定位
3.等待數據接收:腳本會一直等待直到接收到全局地圖數據
4.自動保存並退出:接收到數據後會自動保存並關閉
方法2:Python腳本(ROS2話題方式)
創建 save_global_map.py:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import numpy as np
import open3d as o3d
import os
from datetime import datetime
TOPIC = '/unitree/slam_relocations/global_map' # G1全局地圖話題
class GlobalMapSaver(Node):
def __init__(self):
super().__init__('global_map_saver')
self.save_dir = './global_maps'
self.received = False
os.makedirs(self.save_dir, exist_ok=True)
self.subscription = self.create_subscription(
PointCloud2,
TOPIC,
self.map_callback,
10)
self.get_logger().info('等待接收全局地圖數據...')
def pointcloud2_to_array(self, cloud_msg):
"""將PointCloud2消息轉換為numpy數組"""
# 獲取點雲的字段信息
fmt = []
offset = 0
for field in cloud_msg.fields:
if field.name in ['x', 'y', 'z']:
fmt.append(('x', np.float32, offset))
fmt.append(('y', np.float32, offset + 4))
fmt.append(('z', np.float32, offset + 8))
break
offset += field.count * np.dtype(np.float32).itemsize
# 將數據轉換為numpy數組
cloud_arr = np.frombuffer(cloud_msg.data, dtype=np.float32)
# 重新組織數組形狀
num_points = cloud_msg.width * cloud_msg.height
cloud_arr = cloud_arr.reshape(num_points, -1)
# 提取xyz坐標(通常是前3列)
points = cloud_arr[:, :3]
# 移除NaN值
points = points[~np.any(np.isnan(points), axis=1)]
return points
def map_callback(self, msg):
if self.received:
return
self.received = True
self.get_logger().info('接收到全局地圖數據,,,,開始處理...')
self.get_logger().info(f'數據大小: {len(msg.data)} 字節')
try:
# 方法1:使用自定義轉換
points_array = self.pointcloud2_to_array(msg)
self.get_logger().info(f'提取到 {len(points_array)} 個有效點')
if len(points_array) == 0:
self.get_logger().warning('未找到有效點,,嘗試備用方法...')
# 備用方法:直接解析數據
import struct
point_step = msg.point_step
points_list = []
for i in range(0, len(msg.data), point_step):
try:
x, y, z = struct.unpack_from('fff', msg.data, i)
if not (np.isnan(x) or np.isnan(y) or np.isnan(z)):
points_list.append([x, y, z])
except:
continue
points_array = np.array(points_list, dtype=np.float32)
self.get_logger().info(f'備用方法提取到 {len(points_array)} 個點')
if len(points_array) == 0:
self.get_logger().error('點雲數據為空,,無法保存')
return
# 創建並保存點雲
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points_array)
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
# 保存為PCD
pcd_path = os.path.join(self.save_dir, f"global_map_{timestamp}.pcd")
o3d.io.write_point_cloud(pcd_path, pcd)
# 保存為PLY
ply_path = os.path.join(self.save_dir, f"global_map_{timestamp}.ply")
o3d.io.write_point_cloud(ply_path, pcd)
self.get_logger().info(f'全局地圖已保存:')
self.get_logger().info(f' PCD格式: {pcd_path}')
self.get_logger().info(f' PLY格式: {ply_path}')
self.get_logger().info(f' 點雲數量: {len(points_array)} 個點')
# 顯示點雲範圍
min_coords = np.min(points_array, axis=0)
max_coords = np.max(points_array, axis=0)
self.get_logger().info(f' 點雲範圍: X[{min_coords[0]:.2f}, {max_coords[0]:.2f}], '
f'Y[{min_coords[1]:.2f}, {max_coords[1]:.2f}], '
f'Z[{min_coords[2]:.2f}, {max_coords[2]:.2f}]')
except Exception as e:
self.get_logger().error(f'處理點雲數據時出錯: {str(e)}')
import traceback
self.get_logger().error(traceback.format_exc())
self.get_logger().info('處理完成')
rclpy.shutdown()
def main():
rclpy.init()
map_saver = GlobalMapSaver()
rclpy.spin(map_saver)
if __name__ == '__main__':
main()
安裝依賴
# 安裝必要的Python包
pip install open3d
使用方法
1.運行保存腳本:
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
python3 save_global_map.py
2.開啟定位,,,,使用SLAM導航服務接口開啟定位
3.等待數據接收:腳本會一直等待直到接收到全局地圖數據
4.自動保存並退出:接收到數據後會自動保存並關閉
驗證保存的文件
# 使用pcl_viewer查看保存的點雲(如果安裝了PCL工具)
pcl_viewer global_map_*.pcd