msg_conversion_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <cmath>
20 
21 #include "gtest/gtest.h"
22 #include "sensor_msgs/LaserScan.h"
23 
24 namespace cartographer_ros {
25 namespace {
26 
27 TEST(MsgConversion, LaserScanToPointCloud) {
28  sensor_msgs::LaserScan laser_scan;
29  for (int i = 0; i < 8; ++i) {
30  laser_scan.ranges.push_back(1.f);
31  }
32  laser_scan.angle_min = 0.f;
33  laser_scan.angle_max = 8.f * static_cast<float>(M_PI_4);
34  laser_scan.angle_increment = static_cast<float>(M_PI_4);
35  laser_scan.range_min = 0.f;
36  laser_scan.range_max = 10.f;
37 
38  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
39  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
40  EXPECT_TRUE(point_cloud[1].isApprox(
41  Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
42  EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
43  EXPECT_TRUE(point_cloud[3].isApprox(
44  Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
45  EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
46  EXPECT_TRUE(point_cloud[5].isApprox(
47  Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
48  1e-6));
49  EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
50  EXPECT_TRUE(point_cloud[7].isApprox(
51  Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
52 }
53 
54 TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
55  sensor_msgs::LaserScan laser_scan;
56  laser_scan.ranges.push_back(1.f);
57  laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
58  laser_scan.ranges.push_back(2.f);
59  laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
60  laser_scan.ranges.push_back(3.f);
61  laser_scan.angle_min = 0.f;
62  laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
63  laser_scan.angle_increment = static_cast<float>(M_PI_4);
64  laser_scan.range_min = 2.f;
65  laser_scan.range_max = 10.f;
66 
67  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
68  ASSERT_EQ(2, point_cloud.size());
69  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
70  EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
71 }
72 
73 } // namespace
74 } // namespace cartographer_ros
PointCloudWithIntensities ToPointCloudWithIntensities(const sensor_msgs::LaserScan &msg)
TEST(ActionClientDestruction, persistent_goal_handles_1)


cartographer_ros
Author(s):
autogenerated on Mon Jun 10 2019 12:59:40