34 #include <gtest/gtest.h> 42 pcl::PointCloud<pcl::PointXYZ> pc;
43 for (
float y = -1.0;
y < 1.0;
y += 0.1)
45 for (
float z = -1.0;
z < 1.0;
z += 0.1)
47 pc.push_back(pcl::PointXYZ(0.5,
y,
z));
52 const float hit_range = 0.1 * std::sqrt(3.
f);
54 for (
float y = -0.8;
y < 0.8;
y += 0.11)
56 for (
float z = -0.8;
z < 0.8;
z += 0.13)
58 bool collision =
false;
70 ASSERT_TRUE(collision);
73 for (
float y = -1.0;
y < 1.0;
y += 0.11)
75 for (
float z = -1.0;
z < 1.0;
z += 0.13)
79 bool collision =
false;
85 ASSERT_FALSE(collision);
91 bool collision =
false;
97 ASSERT_FALSE(collision);
101 TEST(Raycast, CollisionTolerance)
103 pcl::PointCloud<pcl::PointXYZ> pc;
104 for (
float y = -1.0;
y < 1.0;
y += 0.05)
106 for (
float z = -1.0;
z < 1.0;
z += 0.1)
108 pc.push_back(pcl::PointXYZ(0.5,
y,
z));
119 bool collision =
false;
128 ASSERT_TRUE(collision);
131 const float hit_range = 0.15 * std::sqrt(3.
f);
136 bool collision =
false;
145 ASSERT_FALSE(collision);
151 pcl::PointCloud<pcl::PointXYZ> pc;
152 for (
float y = -1.0;
y < 1.0;
y += 0.1)
154 for (
float z = -1.0;
z < 1.0;
z += 0.1)
156 pc.push_back(pcl::PointXYZ(0.5,
y,
z));
164 bool collision =
false;
176 ASSERT_TRUE(collision);
179 bool collision =
false;
186 EXPECT_NEAR(point.
sin_angle_, sinf(0.5 / 5.0), 0.05);
191 ASSERT_TRUE(collision);
194 bool collision =
false;
201 EXPECT_NEAR(point.
sin_angle_, sinf(0.5 / 3.0), 0.05);
206 ASSERT_TRUE(collision);
210 int main(
int argc,
char** argv)
212 testing::InitGoogleTest(&argc, argv);
214 return RUN_ALL_TESTS();
std::shared_ptr< ChunkedKdtree > Ptr
void setInputCloud(const typename pcl::PointCloud< POINT_TYPE >::ConstPtr &cloud)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void setRay(typename ChunkedKdtree< POINT_TYPE >::Ptr kdtree, const Vec3 ray_begin, const Vec3 ray_end) final
bool getNextCastResult(CastResult &result) final
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & z() const