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);
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()};
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);
45 if (cloud.header.frame_id !=
"base_link" || cloud.width != 2) {
47 stderr,
"Expected 2 valid points in base_link; got %u points in '%s'\n",
48 cloud.width, cloud.header.frame_id.c_str());
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);
58 if (!std::isfinite(*x) || *x <= 1.0F) {
59 std::fprintf(stderr,
"Expected the later point to use a later TF: %f\n", *x);
63 std::puts(
"LaserProjection applies time-varying TF and filters invalid ranges");