32 #include <geometry_msgs/PoseStamped.h> 35 #include <gtest/gtest.h> 39 static const double EPS = 1e-3;
42 TEST(Tf2Sensor, PointCloud2)
44 sensor_msgs::PointCloud2 cloud;
58 cloud.header.frame_id =
"A";
65 EXPECT_NEAR(*iter_x_after, -9,
EPS);
66 EXPECT_NEAR(*iter_y_after, 18,
EPS);
67 EXPECT_NEAR(*iter_z_after, 27,
EPS);
70 sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->
transform(cloud,
"B",
ros::Time(2.0),
75 EXPECT_NEAR(*iter_x_advanced, -9,
EPS);
76 EXPECT_NEAR(*iter_y_advanced, 18,
EPS);
77 EXPECT_NEAR(*iter_z_advanced, 27,
EPS);
80 int main(
int argc,
char **argv){
81 testing::InitGoogleTest(&argc, argv);
88 geometry_msgs::TransformStamped t;
89 t.transform.translation.x = 10;
90 t.transform.translation.y = 20;
91 t.transform.translation.z = 30;
92 t.transform.rotation.x = 1;
93 t.transform.rotation.y = 0;
94 t.transform.rotation.z = 0;
95 t.transform.rotation.w = 0;
97 t.header.frame_id =
"A";
98 t.child_frame_id =
"B";
101 int ret = RUN_ALL_TESTS();
TEST(Tf2Sensor, PointCloud2)
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
int main(int argc, char **argv)
void setPointCloud2FieldsByString(int n_fields,...)
tf2_ros::Buffer * tf_buffer