34 #include <nav_msgs/Path.h> 35 #include <geometry_msgs/PoseWithCovarianceStamped.h> 37 #include <gtest/gtest.h> 47 nh.
param(
"error_limit", error_limit, 0.3);
49 const boost::function<void(const nav_msgs::Path::ConstPtr&)> cb_path =
50 [&path, &i_path](
const nav_msgs::Path::ConstPtr&
msg) ->
void 54 fprintf(stderr,
"compare_pose: reference received\n");
56 const boost::function<void(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&)> cb_pose =
57 [&path, &i_path, &error_limit](
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&
msg) ->
void 59 if (path.poses.size() > 0 && path.poses[i_path].header.stamp <
ros::Time::now())
61 const float x_error = path.poses[i_path].pose.position.x -
msg->pose.pose.position.x;
62 const float y_error = path.poses[i_path].pose.position.y -
msg->pose.pose.position.y;
63 const float z_error = path.poses[i_path].pose.position.z -
msg->pose.pose.position.z;
64 const float error = std::sqrt(std::pow(x_error, 2) + std::pow(y_error, 2) + std::pow(z_error, 2));
65 const float x_sigma = std::sqrt(
msg->pose.covariance[0 * 6 + 0]);
66 const float y_sigma = std::sqrt(
msg->pose.covariance[1 * 6 + 1]);
67 const float z_sigma = std::sqrt(
msg->pose.covariance[2 * 6 + 2]);
69 fprintf(stderr,
"compare_pose[%lu/%lu]:\n",
70 i_path, path.poses.size());
71 fprintf(stderr,
" position error/limit=%0.3f/%0.3f\n", error, error_limit);
72 fprintf(stderr,
" x error/3sigma=%0.3f/%0.3f\n", x_error, x_sigma * 3.0);
73 fprintf(stderr,
" y error/3sigma=%0.3f/%0.3f\n", y_error, y_sigma * 3.0);
74 fprintf(stderr,
" z error/3sigma=%0.3f/%0.3f\n", z_error, z_sigma * 3.0);
77 if (i_path >= path.poses.size())
80 ASSERT_FALSE(error > error_limit)
81 <<
"Position error is larger then expected.";
82 ASSERT_FALSE(fabs(x_error) > x_sigma * 3.0)
83 <<
"Estimated variance is too small to continue tracking. (x)";
84 ASSERT_FALSE(fabs(y_error) > y_sigma * 3.0)
85 <<
"Estimated variance is too small to continue tracking. (y)";
86 ASSERT_FALSE(fabs(z_error) > z_sigma * 3.0)
87 <<
"Estimated variance is too small to continue tracking. (z)";
101 fprintf(stderr,
"compare_pose finished\n");
104 int main(
int argc,
char** argv)
106 testing::InitGoogleTest(&argc, argv);
109 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)
TEST(ComparePose, Compare)
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()