34 #include <nav_msgs/Path.h> 35 #include <geometry_msgs/PoseWithCovarianceStamped.h> 36 #include <geometry_msgs/TransformStamped.h> 40 #include <gtest/gtest.h> 50 nh.
param(
"cnt_max", cnt_max, 10);
53 const boost::function<void(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&)> cb_pose =
54 [&tfbuf, &cnt, &cnt_max, &tf_ex_cnt](
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&
msg) ->
void 56 geometry_msgs::PoseStamped pose;
59 geometry_msgs::PoseStamped pose_bl;
60 pose_bl.header.frame_id =
"base_link";
61 pose_bl.header.stamp =
msg->header.stamp;
62 pose_bl.pose.orientation.w = 1.0;
63 geometry_msgs::TransformStamped trans =
72 const float x_error = pose.pose.position.x -
msg->pose.pose.position.x;
73 const float y_error = pose.pose.position.y -
msg->pose.pose.position.y;
74 const float z_error = pose.pose.position.z -
msg->pose.pose.position.z;
75 const float error = std::sqrt(std::pow(x_error, 2) + std::pow(y_error, 2) + std::pow(z_error, 2));
77 fprintf(stderr,
"compare_tf[%d/%d]:\n", cnt, cnt_max);
78 fprintf(stderr,
" error=%0.3f\n", error);
84 ASSERT_FALSE(error > 0.05)
85 <<
"tf output diverges from amcl_pose.";
97 ASSERT_FALSE(tf_ex_cnt > 1)
98 <<
"tf exception occures more than once.";
100 fprintf(stderr,
"compare_tf finished\n");
103 int main(
int argc,
char** argv)
105 testing::InitGoogleTest(&argc, argv);
108 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())
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)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
int main(int argc, char **argv)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()