30 #include <geometry_msgs/PointStamped.h>
31 #include <geometry_msgs/TransformStamped.h>
39 #include <gtest/gtest.h>
47 for (uint8_t i = 0; i < 10; ++i)
49 std::this_thread::sleep_for(std::chrono::microseconds(100));
60 TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance)
72 std::vector<std::string> frames;
73 frames.push_back(
"odom");
74 frames.push_back(
"map");
81 geometry_msgs::TransformStamped map_to_odom;
82 map_to_odom.header.stamp =
ros::Time(0, 0);
83 map_to_odom.header.frame_id =
"map";
84 map_to_odom.child_frame_id =
"odom";
85 map_to_odom.transform.translation.x = 0.0;
86 map_to_odom.transform.translation.y = 0.0;
87 map_to_odom.transform.translation.z = 0.0;
88 map_to_odom.transform.rotation.x = 0.0;
89 map_to_odom.transform.rotation.y = 0.0;
90 map_to_odom.transform.rotation.z = 0.0;
91 map_to_odom.transform.rotation.w = 1.0;
94 geometry_msgs::TransformStamped odom_to_base;
95 odom_to_base.header.stamp =
ros::Time(0, 0);
96 odom_to_base.header.frame_id =
"odom";
97 odom_to_base.child_frame_id =
"base";
98 odom_to_base.transform.translation.x = 0.0;
99 odom_to_base.transform.translation.y = 0.0;
100 odom_to_base.transform.translation.z = 0.0;
101 odom_to_base.transform.rotation.x = 0.0;
102 odom_to_base.transform.rotation.y = 0.0;
103 odom_to_base.transform.rotation.z = 0.0;
104 odom_to_base.transform.rotation.w = 1.0;
109 geometry_msgs::PointStamped point;
111 point.header.frame_id =
"base";
121 int main(
int argc,
char **argv){
122 testing::InitGoogleTest(&argc, argv);
123 ros::init(argc, argv,
"tf2_ros_message_filter");
124 return RUN_ALL_TESTS();