00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <tf2_ros/transform_broadcaster.h>
00032 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00033 #include <nav_msgs/Path.h>
00034
00035 #include <algorithm>
00036 #include <string>
00037
00038 #include <gtest/gtest.h>
00039
00040 TEST(TrajectoryRecorder, TfToPath)
00041 {
00042 ros::NodeHandle nh("");
00043
00044 nav_msgs::Path::ConstPtr path;
00045 const boost::function<void(const nav_msgs::Path::ConstPtr&)> cb_path =
00046 [&path](const nav_msgs::Path::ConstPtr& msg) -> void
00047 {
00048 path = msg;
00049 };
00050 ros::Subscriber sub_path = nh.subscribe("path", 1, cb_path);
00051 tf2_ros::TransformBroadcaster tfb;
00052
00053 const tf2::Transform points[] =
00054 {
00055 tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0)),
00056 tf2::Transform(tf2::Quaternion(0, 0, 1, 0), tf2::Vector3(2, 0, 0)),
00057 tf2::Transform(tf2::Quaternion(0, 0, 0, -1), tf2::Vector3(3, 5, 0)),
00058 tf2::Transform(tf2::Quaternion(0, 0, -1, 0), tf2::Vector3(-1, 5, 1))
00059 };
00060 const size_t len = sizeof(points) / sizeof(tf2::Transform);
00061
00062 ros::Duration(1.0).sleep();
00063 for (auto& p : points)
00064 {
00065 for (size_t i = 0; i < 3; ++i)
00066 {
00067 geometry_msgs::TransformStamped trans =
00068 tf2::toMsg(tf2::Stamped<tf2::Transform>(
00069 p, ros::Time::now() + ros::Duration(0.1), "map"));
00070 trans.child_frame_id = "base_link";
00071 tfb.sendTransform(trans);
00072 ros::Duration(0.1).sleep();
00073 }
00074 }
00075 ros::spinOnce();
00076 ASSERT_TRUE(static_cast<bool>(path));
00077 sub_path.shutdown();
00078
00079 ASSERT_EQ(path->poses.size(), len);
00080 for (size_t i = 0; i < len; ++i)
00081 {
00082 ASSERT_EQ(path->poses[i].pose.position.x, points[i].getOrigin().x());
00083 ASSERT_EQ(path->poses[i].pose.position.y, points[i].getOrigin().y());
00084 ASSERT_EQ(path->poses[i].pose.position.z, points[i].getOrigin().z());
00085 ASSERT_EQ(path->poses[i].pose.orientation.x, points[i].getRotation().x());
00086 ASSERT_EQ(path->poses[i].pose.orientation.y, points[i].getRotation().y());
00087 ASSERT_EQ(path->poses[i].pose.orientation.z, points[i].getRotation().z());
00088 ASSERT_EQ(path->poses[i].pose.orientation.w, points[i].getRotation().w());
00089 }
00090 }
00091
00092 int main(int argc, char** argv)
00093 {
00094 testing::InitGoogleTest(&argc, argv);
00095 ros::init(argc, argv, "test_trajectory_recorder");
00096
00097 return RUN_ALL_TESTS();
00098 }