auto_init_403_test.cpp
Go to the documentation of this file.
1 /*
2 Test to verify that when the map is initialized from
3 a single fiducial that the robot's pose is identity
4 and the map entry for the fiducial is reasonable
5 */
6 
7 #include <gtest/gtest.h>
8 
9 #include <ros/ros.h>
11 #include <sensor_msgs/CameraInfo.h>
12 #include <opencv2/highgui/highgui.hpp>
13 #include <cv_bridge/cv_bridge.h>
14 
15 #include <geometry_msgs/PoseWithCovarianceStamped.h>
16 
17 #include <fiducial_msgs/FiducialMapEntry.h>
18 #include <fiducial_msgs/FiducialMapEntryArray.h>
19 
20 
21 class AutoInitTest : public ::testing::Test {
22 protected:
23  virtual void SetUp() {
25  image_pub = it->advertise("camera/image", 1);
26 
27  CameraInfoPub = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 5);
28 
29  c_info.header.frame_id = "camera";
30  c_info.height = 960;
31  c_info.width = 1280;
32  c_info.distortion_model = "plumb_bob";
33 
34  double data_D[] = {0.1349735087283542, -0.2335869827451621,
35  0.0006697030315075139, 0.004846737465872353, 0.0};
36 
37  c_info.D = std::vector<double> (data_D,
38  data_D + sizeof(data_D)/ sizeof(double));
39 
40  c_info.K = {1006.126285753055, 0.0, 655.8639244150409,
41  0.0, 1004.015433012594, 490.6140221242933,
42  0.0, 0.0, 1.0};
43 
44  c_info.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
45 
46  c_info.P = {1021.54345703125, 0.0, 661.9091982335958, 0.0,
47  0.0, 1025.251953125, 490.6380671707448, 0.0,
48  0.0, 0.0, 1.0, 0.0};
49 
50  ros::NodeHandle nh_priv("~");
51  nh_priv.getParam("image_directory", image_directory);
52 
53  pose_sub = nh.subscribe("/fiducial_pose", 1,
55  got_pose = false;
56 
57  fiducial_msgs::FiducialMapEntryArray map;
58 
59  map_sub = nh.subscribe("/fiducial_map", 1,
61  got_map = false;
62  }
63 
64  virtual void TearDown() { delete it;}
65 
66  void publish_image(std::string file)
67  {
68  cv::Mat image = cv::imread(image_directory+file, CV_LOAD_IMAGE_COLOR);
69  cv::waitKey(30);
70  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),
71  "bgr8", image).toImageMsg();
72  image_pub.publish(msg);
73  c_info.header.stamp = ros::Time::now();
75  }
76 
77  void map_callback(const fiducial_msgs::FiducialMapEntryArray& msg)
78  {
79  got_map = true;
80  map = msg;
81  }
82 
83  void pose_callback(const geometry_msgs::PoseWithCovarianceStamped& msg)
84  {
85  got_pose = true;
86  pose = msg;
87  }
88 
90 
91  // Set up Publishing of static images
94 
95  sensor_msgs::CameraInfo c_info;
97 
98  std::string image_directory;
99 
102 
103  bool got_map;
104  bool got_pose;
105 
106  fiducial_msgs::FiducialMapEntryArray map;
107  geometry_msgs::PoseWithCovarianceStamped pose;
108 
109 };
110 
111 TEST_F(AutoInitTest, tag_403_d7_14cm) {
112  ros::Rate loop_rate(5);
113  while (nh.ok() && (!got_pose || !got_map)) {
114  publish_image("403.jpg");
115  ros::spinOnce();
116  loop_rate.sleep();
117  }
118 
119  ASSERT_NEAR(0, pose.pose.pose.position.x, 0.001);
120  ASSERT_NEAR(0, pose.pose.pose.position.y, 0.001);
121  ASSERT_NEAR(0, pose.pose.pose.position.z, 0.001);
122 
123  ASSERT_NEAR(1, pose.pose.pose.orientation.w, 0.001);
124  ASSERT_NEAR(0, pose.pose.pose.orientation.x, 0.001);
125  ASSERT_NEAR(0, pose.pose.pose.orientation.y, 0.001);
126  ASSERT_NEAR(0, pose.pose.pose.orientation.z, 0.001);
127 
128  ASSERT_LE(1, map.fiducials.size());
129 
130  const fiducial_msgs::FiducialMapEntry &fid = map.fiducials[0];
131  ASSERT_EQ(403, fid.fiducial_id);
132  ASSERT_NEAR(0.7611, fid.x, 0.001);
133  ASSERT_NEAR(0.2505, fid.y, 0.001);
134  ASSERT_NEAR(0.4028, fid.z, 0.001);
135  ASSERT_NEAR(1.5751, fid.rx, 0.001);
136  ASSERT_NEAR(-0.014, fid.ry, 0.001);
137  ASSERT_NEAR(-1.546, fid.rz, 0.001);
138 }
139 
140 int main(int argc, char** argv)
141 {
142 
143  testing::InitGoogleTest(&argc, argv);
144  ros::init(argc, argv, "AutoInitTest");
145  return RUN_ALL_TESTS();
146 }
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void publish_image(std::string file)
ros::NodeHandle nh
virtual void TearDown()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
void pose_callback(const geometry_msgs::PoseWithCovarianceStamped &msg)
std::string image_directory
TEST_F(AutoInitTest, tag_403_d7_14cm)
void map_callback(const fiducial_msgs::FiducialMapEntryArray &msg)
ros::Subscriber map_sub
sensor_msgs::CameraInfo c_info
void publish(const sensor_msgs::Image &message) const
fiducial_msgs::FiducialMapEntryArray map
image_transport::Publisher image_pub
image_transport::ImageTransport * it
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
ros::Publisher CameraInfoPub
virtual void SetUp()
bool getParam(const std::string &key, std::string &s) const
geometry_msgs::PoseWithCovarianceStamped pose
static Time now()
bool ok() const
ROSCPP_DECL void spinOnce()
sensor_msgs::ImagePtr toImageMsg() const
ros::Subscriber pose_sub


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Tue Jun 1 2021 03:03:29