servo_lidar_3d
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
13
class
PointCloudToPcd
final :
public
rclcpp::Node {
14
public
:
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
25
private
:
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
57
int
main
(
int
argc,
char
** argv) {
58
rclcpp::init(argc, argv);
59
rclcpp::spin(std::make_shared<PointCloudToPcd>());
60
rclcpp::shutdown();
61
return
0;
62
}
PointCloudToPcd::PointCloudToPcd
PointCloudToPcd()
定義
pointcloud_to_pcd.cpp:15
main
int main(int argc, char **argv)
定義
pointcloud_to_pcd.cpp:57
src
pointcloud_to_pcd.cpp
構築:
1.18.0