34 #include <geometry_msgs/TransformStamped.h> 39 #include <gtest/gtest.h> 55 projected_frame_ = std::string(GetParam());
63 geometry_msgs::TransformStamped out;
77 const tf2::Vector3 result = trans * tf2::Vector3(1, 2, 3);
78 ASSERT_NEAR(out.transform.translation.x, result[0], 1e-4);
82 ASSERT_NEAR(out.transform.translation.x, 1, 1e-4);
84 ASSERT_NEAR(out.transform.translation.y, 2, 1e-4);
85 ASSERT_NEAR(out.transform.translation.z, 0, 1e-4);
86 ASSERT_NEAR(out.transform.rotation.x, 0, 1e-4);
87 ASSERT_NEAR(out.transform.rotation.y, 0, 1e-4);
88 ASSERT_NEAR(out.transform.rotation.z, 0.7071, 1e-4);
89 ASSERT_NEAR(out.transform.rotation.w, 0.7071, 1e-4);
90 ASSERT_EQ(out.header.frame_id,
"map");
97 "base_link_projected",
98 "base_link_projected2",
99 "base_link_tilt_projected",
100 "base_link_tilt_projected_with_aligned"));
102 int main(
int argc,
char** argv)
104 testing::InitGoogleTest(&argc, argv);
105 ros::init(argc, argv,
"test_tf_projection_node");
107 return RUN_ALL_TESTS();
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf2_ros::TransformListener tfl_
std::string projected_frame_
INSTANTIATE_TEST_CASE_P(ProjectionTransformInstance, TfProjectionTest,::testing::Values("base_link_projected","base_link_projected2","base_link_tilt_projected","base_link_tilt_projected_with_aligned"))
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
int main(int argc, char **argv)
TEST_P(TfProjectionTest, ProjectionTransform)