servo_lidar_3d
読み取り中…
検索中…
一致する文字列を見つけられません
pointcloud_to_pcd.cpp
[詳解]
1#include <chrono>
2#include <filesystem>
3#include <fstream>
4#include <functional>
5#include <iomanip>
6#include <memory>
7#include <string>
8
9#include "rclcpp/rclcpp.hpp"
10#include "sensor_msgs/msg/point_cloud2.hpp"
11#include "sensor_msgs/point_cloud2_iterator.hpp"
12
13class PointCloudToPcd final : public rclcpp::Node {
14public:
15 PointCloudToPcd() : Node("pointcloud_to_pcd") {
16 topic_ = declare_parameter("topic", std::string("/pointcloud"));
17 output_dir_ = declare_parameter("output_dir", std::string("/tmp/servo_lidar_3d_pcd"));
18 std::filesystem::create_directories(output_dir_);
19 sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
20 topic_, rclcpp::QoS(10).reliable(),
21 std::bind(&PointCloudToPcd::callback, this, std::placeholders::_1));
22 RCLCPP_INFO(get_logger(), "Saving %s to %s", topic_.c_str(), output_dir_.c_str());
23 }
24
25private:
26 void callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
27 const auto stamp = std::chrono::system_clock::now().time_since_epoch();
28 const auto stamp_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(stamp).count();
29 const auto path = std::filesystem::path(output_dir_) /
30 ("cloud_" + std::to_string(stamp_ns) + ".pcd");
31 std::ofstream file(path);
32 if (!file) {
33 RCLCPP_ERROR(get_logger(), "Cannot write %s", path.c_str());
34 return;
35 }
36
37 file << "# .PCD v0.7 - Point Cloud Data file format\n"
38 << "VERSION 0.7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\n"
39 << "COUNT 1 1 1\nWIDTH " << msg->width * msg->height << "\nHEIGHT 1\n"
40 << "VIEWPOINT 0 0 0 1 0 0 0\nPOINTS " << msg->width * msg->height
41 << "\nDATA ascii\n" << std::setprecision(8);
42
43 sensor_msgs::PointCloud2ConstIterator<float> x(*msg, "x");
44 sensor_msgs::PointCloud2ConstIterator<float> y(*msg, "y");
45 sensor_msgs::PointCloud2ConstIterator<float> z(*msg, "z");
46 for (std::size_t i = 0; i < msg->width * msg->height; ++i, ++x, ++y, ++z) {
47 file << *x << ' ' << *y << ' ' << *z << '\n';
48 }
49 RCLCPP_INFO(get_logger(), "Saved %zu points to %s",
50 static_cast<std::size_t>(msg->width) * msg->height, path.c_str());
51 }
52
53 std::string topic_, output_dir_;
54 rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
55};
56
57int main(int argc, char ** argv) {
58 rclcpp::init(argc, argv);
59 rclcpp::spin(std::make_shared<PointCloudToPcd>());
60 rclcpp::shutdown();
61 return 0;
62}
int main(int argc, char **argv)