test_debug_output.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-2019, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 
32 #include <nav_msgs/Odometry.h>
33 #include <sensor_msgs/Imu.h>
34 #include <sensor_msgs/PointCloud2.h>
36 #include <std_srvs/Trigger.h>
37 #include <tf2/utils.h>
39 
40 #include <random>
41 #include <vector>
42 
43 #include <gtest/gtest.h>
44 
45 namespace
46 {
47 void generateSamplePointcloud2(
48  sensor_msgs::PointCloud2& cloud,
49  const float offset_x,
50  const float offset_y,
51  const float offset_z)
52 {
53  cloud.height = 1;
54  cloud.is_bigendian = false;
55  cloud.is_dense = false;
56  sensor_msgs::PointCloud2Modifier modifier(cloud);
57  modifier.setPointCloud2Fields(
58  4,
59  "x", 1, sensor_msgs::PointField::FLOAT32,
60  "y", 1, sensor_msgs::PointField::FLOAT32,
61  "z", 1, sensor_msgs::PointField::FLOAT32,
62  "intensity", 1, sensor_msgs::PointField::FLOAT32);
63 
64  class Point
65  {
66  public:
67  float x_, y_, z_;
68  Point(const float x, const float y, const float z)
69  : x_(x)
70  , y_(y)
71  , z_(z)
72  {
73  }
74  };
75  std::vector<Point> points;
76  // Draw cube
77  for (float x = -1; x < 1; x += 0.05)
78  {
79  for (float y = -1; y < 1; y += 0.05)
80  {
81  points.push_back(Point(1.0 / 2 + offset_x, y + offset_y, x + offset_z));
82  points.push_back(Point(-1.0 / 2 + offset_x, y + offset_y, x + offset_z));
83  }
84  }
85 
86  modifier.resize(points.size());
87  cloud.width = points.size();
88  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
89  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
90  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
91 
92  for (const Point& p : points)
93  {
94  *iter_x = p.x_;
95  *iter_y = p.y_;
96  *iter_z = p.z_;
97  ++iter_x;
98  ++iter_y;
99  ++iter_z;
100  }
101 }
102 
103 inline sensor_msgs::PointCloud2 generateMapMsg(
104  const float offset_x,
105  const float offset_y,
106  const float offset_z)
107 {
108  sensor_msgs::PointCloud2 cloud;
109  generateSamplePointcloud2(cloud, offset_x, offset_y, offset_z);
110  cloud.header.frame_id = "map";
111  return cloud;
112 }
113 inline sensor_msgs::PointCloud2 generateCloudMsg()
114 {
115  sensor_msgs::PointCloud2 cloud;
116  generateSamplePointcloud2(cloud, 0, 0, 0);
117  cloud.header.frame_id = "base_link";
118  cloud.header.stamp = ros::Time::now();
119  return cloud;
120 }
121 inline sensor_msgs::Imu generateImuMsg()
122 {
123  sensor_msgs::Imu imu;
124  imu.header.frame_id = "base_link";
125  imu.header.stamp = ros::Time::now();
126  imu.orientation.w = 1;
127  imu.linear_acceleration.z = 9.8;
128  return imu;
129 }
130 inline nav_msgs::Odometry generateOdomMsg()
131 {
132  nav_msgs::Odometry odom;
133  odom.header.frame_id = "odom";
134  odom.header.stamp = ros::Time::now();
135  odom.pose.pose.position.x = 1;
136  odom.pose.pose.orientation.w = 1;
137  return odom;
138 }
139 } // namespace
140 
141 TEST(DebugOutput, MatchedUnmatched)
142 {
143  sensor_msgs::PointCloud2::ConstPtr matched, unmatched;
144 
145  const boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)> cb_matched =
146  [&matched](const sensor_msgs::PointCloud2::ConstPtr& msg) -> void
147  {
148  matched = msg;
149  };
150  const boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)> cb_unmatched =
151  [&unmatched](const sensor_msgs::PointCloud2::ConstPtr& msg) -> void
152  {
153  unmatched = msg;
154  };
155 
156  ros::NodeHandle nh("");
157  ros::Subscriber sub_matched = nh.subscribe("mcl_3dl/matched", 1, cb_matched);
158  ros::Subscriber sub_unmatched = nh.subscribe("mcl_3dl/unmatched", 1, cb_unmatched);
159 
160  ros::Publisher pub_mapcloud = nh.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
161  ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
162  ros::Publisher pub_imu = nh.advertise<sensor_msgs::Imu>("imu/data", 1);
163  ros::Publisher pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 1);
164 
165  const float offset_x = 1;
166  const float offset_y = 0;
167  const float offset_z = 0;
168  pub_mapcloud.publish(generateMapMsg(offset_x, offset_y, offset_z));
169 
170  ros::Duration(1.0).sleep();
171  ros::Rate rate(10);
172  for (int i = 0; i < 40; ++i)
173  {
174  rate.sleep();
175  ros::spinOnce();
176  if (matched && unmatched)
177  break;
178  pub_cloud.publish(generateCloudMsg());
179  pub_imu.publish(generateImuMsg());
180  pub_odom.publish(generateOdomMsg());
181  }
182  ASSERT_TRUE(ros::ok());
183 
184  ASSERT_TRUE(static_cast<bool>(matched));
185  ASSERT_TRUE(static_cast<bool>(unmatched));
186 
187  ASSERT_GT(matched->width * matched->height, 0u);
188  ASSERT_GT(unmatched->width * unmatched->height, 0u);
189 
190  {
194  for (; x != x.end(); ++x, ++y, ++z)
195  {
196  ASSERT_NEAR(*x, 0.5f, 0.1f);
197  ASSERT_TRUE(-1.1 < *y && *y < 1.1);
198  ASSERT_TRUE(-1.1 < *z && *z < 1.1);
199  }
200  }
201  {
205  for (; x != x.end(); ++x, ++y, ++z)
206  {
207  ASSERT_NEAR(*x, -0.5f, 0.1f);
208  ASSERT_TRUE(-1.1 < *y && *y < 1.1);
209  ASSERT_TRUE(-1.1 < *z && *z < 1.1);
210  }
211  }
212 }
213 
214 int main(int argc, char** argv)
215 {
216  testing::InitGoogleTest(&argc, argv);
217  ros::init(argc, argv, "test_debug_output");
218 
219  return RUN_ALL_TESTS();
220 }
msg
void publish(const boost::shared_ptr< M > &message) const
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & x() const
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & z() const
TEST(DebugOutput, MatchedUnmatched)
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
static Time now()
ROSCPP_DECL void spinOnce()


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29