31 #include <sensor_msgs/PointCloud2.h> 36 #include <gtest/gtest.h> 43 -
TOLERANCE <= y && y <= 0.1 + TOLERANCE &&
44 -TOLERANCE <= z && z <= 0.001 +
TOLERANCE;
49 -
TOLERANCE <= y && y <= 0.1 + TOLERANCE &&
52 bool isOnCorner(
const float x,
const float y,
const float z)
55 -
TOLERANCE <= y && y <= 0.1 + TOLERANCE &&
59 TEST(ObjToPointCloud, PointCloud)
62 sensor_msgs::PointCloud2::ConstPtr cloud;
64 const boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)> cb =
65 [&cloud](
const sensor_msgs::PointCloud2::ConstPtr& msg) ->
void 72 for (
int i = 0; i < 30 &&
ros::ok(); ++i)
79 ASSERT_TRUE(static_cast<bool>(cloud));
81 cloud->width * cloud->height,
82 lround((0.5 * 0.1 + 0.1 * 0.2) / (0.05 * 0.05)),
86 iter_x != iter_x.
end();
87 ++iter_x, ++iter_y, ++iter_z)
93 << *iter_x <<
", " << *iter_y <<
", " << *iter_z <<
" is not on the expected surface";
97 int main(
int argc,
char** argv)
99 testing::InitGoogleTest(&argc, argv);
100 ros::init(argc, argv,
"test_obj_to_pointcloud");
102 return RUN_ALL_TESTS();
constexpr float TOLERANCE
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool isOnCorner(const float x, const float y, const float z)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool isOnBottomSurface(const float x, const float y, const float z)
int main(int argc, char **argv)
TEST(ObjToPointCloud, PointCloud)
bool isOnSideSurface(const float x, const float y, const float z)
PointCloud2ConstIterator< T > end() const
ROSCPP_DECL void spinOnce()