nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
test_laser_projection.cpp
[詳解]
1#include <cmath>
2#include <cstdio>
3#include <cstdlib>
4#include <limits>
5
6#include "geometry_msgs/msg/transform_stamped.hpp"
7#include "laser_geometry/laser_geometry.hpp"
8#include "sensor_msgs/msg/laser_scan.hpp"
9#include "sensor_msgs/msg/point_cloud2.hpp"
10#include "sensor_msgs/point_cloud2_iterator.hpp"
11#include "tf2/buffer_core.h"
12
13int main()
14{
15 tf2::BufferCore tf(tf2::durationFromSec(10.0));
16 for (int sec : {1, 3}) {
17 geometry_msgs::msg::TransformStamped transform;
18 transform.header.stamp.sec = sec;
19 transform.header.frame_id = "base_link";
20 transform.child_frame_id = "laser";
21 transform.transform.translation.x = static_cast<double>(sec - 1);
22 transform.transform.rotation.w = 1.0;
23 if (!tf.setTransform(transform, "test", false)) {
24 std::fprintf(stderr, "Failed to add test transform at %d seconds\n", sec);
25 return EXIT_FAILURE;
26 }
27 }
28
29 sensor_msgs::msg::LaserScan scan;
30 scan.header.stamp.sec = 1;
31 scan.header.frame_id = "laser";
32 scan.angle_min = 0.0F;
33 scan.angle_increment = 0.0F;
34 scan.time_increment = 0.5F;
35 scan.scan_time = 1.0F;
36 scan.range_min = 0.1F;
37 scan.range_max = 10.0F;
38 scan.ranges = {1.0F, 1.0F, std::numeric_limits<float>::quiet_NaN()};
39
40 sensor_msgs::msg::PointCloud2 cloud;
41 laser_geometry::LaserProjection projector;
42 projector.transformLaserScanToPointCloud(
43 "base_link", scan, cloud, tf, -1.0, laser_geometry::channel_option::None);
44
45 if (cloud.header.frame_id != "base_link" || cloud.width != 2) {
46 std::fprintf(
47 stderr, "Expected 2 valid points in base_link; got %u points in '%s'\n",
48 cloud.width, cloud.header.frame_id.c_str());
49 return EXIT_FAILURE;
50 }
51
52 sensor_msgs::PointCloud2ConstIterator<float> x(cloud, "x");
53 if (!std::isfinite(*x) || std::fabs(*x - 1.0F) > 1e-4F) {
54 std::fprintf(stderr, "Unexpected first point x coordinate: %f\n", *x);
55 return EXIT_FAILURE;
56 }
57 ++x;
58 if (!std::isfinite(*x) || *x <= 1.0F) {
59 std::fprintf(stderr, "Expected the later point to use a later TF: %f\n", *x);
60 return EXIT_FAILURE;
61 }
62
63 std::puts("LaserProjection applies time-varying TF and filters invalid ranges");
64 return EXIT_SUCCESS;
65}
int main(int argc, char **argv)