33 #include <nav_msgs/Path.h> 38 #include <gtest/gtest.h> 40 TEST(TrajectoryRecorder, TfToPath)
44 nav_msgs::Path::ConstPtr path;
45 const boost::function<void(const nav_msgs::Path::ConstPtr&)> cb_path =
46 [&path](
const nav_msgs::Path::ConstPtr& msg) ->
void 63 for (
auto& p : points)
65 for (
size_t i = 0; i < 3; ++i)
67 geometry_msgs::TransformStamped trans =
70 trans.child_frame_id =
"base_link";
76 ASSERT_TRUE(static_cast<bool>(path));
79 ASSERT_EQ(path->poses.size(), len);
80 for (
size_t i = 0; i < len; ++i)
82 ASSERT_EQ(path->poses[i].pose.position.x, points[i].getOrigin().x());
83 ASSERT_EQ(path->poses[i].pose.position.y, points[i].getOrigin().y());
84 ASSERT_EQ(path->poses[i].pose.position.z, points[i].getOrigin().z());
85 ASSERT_EQ(path->poses[i].pose.orientation.x, points[i].getRotation().x());
86 ASSERT_EQ(path->poses[i].pose.orientation.y, points[i].getRotation().y());
87 ASSERT_EQ(path->poses[i].pose.orientation.z, points[i].getRotation().z());
88 ASSERT_EQ(path->poses[i].pose.orientation.w, points[i].getRotation().w());
92 int main(
int argc,
char** argv)
94 testing::InitGoogleTest(&argc, argv);
95 ros::init(argc, argv,
"test_trajectory_recorder");
97 return RUN_ALL_TESTS();
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)