33 #include <nav_msgs/Path.h> 34 #include <std_srvs/Empty.h> 39 #include <gtest/gtest.h> 41 TEST(TrajectoryRecorder, TfToPath)
45 nav_msgs::Path::ConstPtr path;
46 int received_count = 0;
47 const boost::function<void(const nav_msgs::Path::ConstPtr&)> cb_path =
48 [&path, &received_count](
const nav_msgs::Path::ConstPtr& msg) ->
void 66 for (
auto& p : points)
68 for (
size_t i = 0; i < 3; ++i)
70 geometry_msgs::TransformStamped trans =
73 trans.child_frame_id =
"base_link";
79 ASSERT_TRUE(static_cast<bool>(path));
80 ASSERT_EQ(received_count, 1);
82 ASSERT_EQ(path->poses.size(), len);
83 for (
size_t i = 0; i < len; ++i)
85 ASSERT_EQ(path->poses[i].pose.position.x, points[i].getOrigin().x());
86 ASSERT_EQ(path->poses[i].pose.position.y, points[i].getOrigin().y());
87 ASSERT_EQ(path->poses[i].pose.position.z, points[i].getOrigin().z());
88 ASSERT_EQ(path->poses[i].pose.orientation.x, points[i].getRotation().x());
89 ASSERT_EQ(path->poses[i].pose.orientation.y, points[i].getRotation().y());
90 ASSERT_EQ(path->poses[i].pose.orientation.z, points[i].getRotation().z());
91 ASSERT_EQ(path->poses[i].pose.orientation.w, points[i].getRotation().w());
95 std_srvs::Empty empty;
96 ASSERT_TRUE(client.call(empty));
98 while (received_count != 2)
103 ASSERT_EQ(static_cast<int>(path->poses.size()), 1);
104 ASSERT_EQ(path->poses.back().pose.position.x, points[len - 1].getOrigin().x());
105 ASSERT_EQ(path->poses.back().pose.position.y, points[len - 1].getOrigin().y());
106 ASSERT_EQ(path->poses.back().pose.position.z, points[len - 1].getOrigin().z());
107 ASSERT_EQ(path->poses.back().pose.orientation.x, points[len - 1].getRotation().x());
108 ASSERT_EQ(path->poses.back().pose.orientation.y, points[len - 1].getRotation().y());
109 ASSERT_EQ(path->poses.back().pose.orientation.z, points[len - 1].getRotation().z());
110 ASSERT_EQ(path->poses.back().pose.orientation.w, points[len - 1].getRotation().w());
113 int main(
int argc,
char** argv)
115 testing::InitGoogleTest(&argc, argv);
116 ros::init(argc, argv,
"test_trajectory_recorder");
118 return RUN_ALL_TESTS();
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
TEST(TrajectoryRecorder, TfToPath)