test_global_localization.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, 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 <geometry_msgs/PoseArray.h>
33 #include <mcl_3dl_msgs/Status.h>
34 #include <nav_msgs/Odometry.h>
35 #include <sensor_msgs/Imu.h>
36 #include <sensor_msgs/PointCloud2.h>
38 #include <std_srvs/Trigger.h>
39 #include <tf2/utils.h>
41 
42 #include <random>
43 #include <vector>
44 
45 #include <gtest/gtest.h>
46 
47 namespace
48 {
49 void generateSamplePointcloud2(
50  sensor_msgs::PointCloud2& cloud,
51  const float x0,
52  const float y0,
53  const float x1,
54  const float y1,
55  const float offset_x,
56  const float offset_y,
57  const float offset_z,
58  const float offset_yaw)
59 {
60  std::random_device seed;
61  std::default_random_engine engine(seed());
62  std::normal_distribution<float> rand(0, 0.01);
63 
64  cloud.height = 1;
65  cloud.is_bigendian = false;
66  cloud.is_dense = false;
67  sensor_msgs::PointCloud2Modifier modifier(cloud);
68  modifier.setPointCloud2Fields(
69  4,
70  "x", 1, sensor_msgs::PointField::FLOAT32,
71  "y", 1, sensor_msgs::PointField::FLOAT32,
72  "z", 1, sensor_msgs::PointField::FLOAT32,
73  "intensity", 1, sensor_msgs::PointField::FLOAT32);
74 
75  class Point
76  {
77  public:
78  float x_, y_, z_;
79  Point(const float x, const float y, const float z)
80  : x_(x)
81  , y_(y)
82  , z_(z)
83  {
84  }
85  };
86  std::vector<Point> points;
87  const float grid_xy = 0.15;
88  const float grid_z = 0.1;
89  const float floor_size = 6.0;
90  // Draw floor
91  for (float x = -floor_size; x < floor_size; x += grid_xy)
92  for (float y = -floor_size; y < floor_size; y += grid_xy)
93  if (x0 < x && x < x1 && y0 < y && y < y1)
94  points.push_back(Point(x, y, 0.0));
95  // Draw objects
96  for (float x = -2; x < 1.6; x += grid_xy)
97  if (x0 < x && x < x1)
98  for (float z = 0.5; z < 1.5; z += grid_z)
99  points.push_back(Point(x, 1.6, z));
100  for (float y = -0.5; y < 1.6; y += grid_xy)
101  if (y0 < y && y < y1)
102  for (float z = 0.5; z < 2.0; z += grid_z)
103  points.push_back(Point(1.6, y, z));
104  for (float x = 0; x < 1.6; x += grid_xy)
105  if (x0 < x && x < x1)
106  for (float z = 0.5; z < 2.0; z += grid_z)
107  points.push_back(Point(x, -2.1 + x, z));
108 
109  const float o_cos = cosf(offset_yaw);
110  const float o_sin = sinf(offset_yaw);
111  modifier.resize(points.size());
112  cloud.width = points.size();
113  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
114  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
115  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
116 
117  for (const Point& p : points)
118  {
119  *iter_x = p.x_ * o_cos - p.y_ * o_sin + offset_x + rand(engine);
120  *iter_y = p.x_ * o_sin + p.y_ * o_cos + offset_y + rand(engine);
121  *iter_z = p.z_ + offset_z + rand(engine);
122  ++iter_x;
123  ++iter_y;
124  ++iter_z;
125  }
126 }
127 
128 inline sensor_msgs::PointCloud2 generateMapMsg()
129 {
130  sensor_msgs::PointCloud2 cloud;
131  generateSamplePointcloud2(cloud, -100, -100, 100, 100, 0, 0, 0, 0);
132  cloud.header.frame_id = "map";
133  return cloud;
134 }
135 inline sensor_msgs::PointCloud2 generateCloudMsg(
136  const float offset_x,
137  const float offset_y,
138  const float offset_z,
139  const float offset_yaw)
140 {
141  sensor_msgs::PointCloud2 cloud;
142  generateSamplePointcloud2(
143  cloud, -2, -2, 2, 2,
144  offset_x, offset_y, offset_z, offset_yaw);
145  cloud.header.frame_id = "laser";
146  cloud.header.stamp = ros::Time::now();
147  return cloud;
148 }
149 inline sensor_msgs::Imu generateImuMsg()
150 {
151  sensor_msgs::Imu imu;
152  imu.header.frame_id = "base_link";
153  imu.header.stamp = ros::Time::now();
154  imu.orientation.w = 1;
155  imu.linear_acceleration.z = 9.8;
156  return imu;
157 }
158 inline nav_msgs::Odometry generateOdomMsg()
159 {
160  nav_msgs::Odometry odom;
161  odom.header.frame_id = "odom";
162  odom.header.stamp = ros::Time::now();
163  odom.pose.pose.position.y = 5;
164  odom.pose.pose.orientation.w = 1;
165  return odom;
166 }
167 } // namespace
168 
169 TEST(GlobalLocalization, Localize)
170 {
171  geometry_msgs::PoseArray::ConstPtr poses;
172  mcl_3dl_msgs::Status::ConstPtr status;
173 
174  const boost::function<void(const geometry_msgs::PoseArray::ConstPtr&)> cb_pose =
175  [&poses](const geometry_msgs::PoseArray::ConstPtr& msg) -> void
176  {
177  poses = msg;
178  };
179  const boost::function<void(const mcl_3dl_msgs::Status::ConstPtr&)> cb_status =
180  [&status](const mcl_3dl_msgs::Status::ConstPtr& msg) -> void
181  {
182  status = msg;
183  };
184 
185  ros::NodeHandle nh("");
186  ros::Subscriber sub_pose = nh.subscribe("mcl_3dl/particles", 1, cb_pose);
187  ros::Subscriber sub_status = nh.subscribe("mcl_3dl/status", 1, cb_status);
188 
189  ros::ServiceClient src_global_localization =
190  nh.serviceClient<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(
191  "global_localization");
192 
193  ros::Publisher pub_mapcloud = nh.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
194  ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
195  ros::Publisher pub_imu = nh.advertise<sensor_msgs::Imu>("imu/data", 1);
196  ros::Publisher pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 1);
197 
198  pub_mapcloud.publish(generateMapMsg());
199 
200  for (float offset_x = -0.5; offset_x <= 0.51; offset_x += 1.0)
201  {
202  for (float offset_yaw = -M_PI / 2; offset_yaw <= M_PI / 2 + 0.1; offset_yaw += M_PI)
203  {
204  const float laser_frame_height = 0.5;
205  const float offset_y = 0.54;
206  const float offset_z = 0.0;
207 
208  ros::Rate rate(10);
209  // Wait until mcl_3dl initialization
210  while (ros::ok())
211  {
212  rate.sleep();
213  ros::spinOnce();
214  if (status && status->status == mcl_3dl_msgs::Status::NORMAL)
215  break;
216  pub_cloud.publish(
217  generateCloudMsg(offset_x, offset_y, offset_z - laser_frame_height, offset_yaw));
218  pub_imu.publish(generateImuMsg());
219  pub_odom.publish(generateOdomMsg());
220  }
221  ASSERT_TRUE(ros::ok());
222 
223  std_srvs::Trigger trigger;
224  ASSERT_TRUE(src_global_localization.call(trigger));
225  ros::spinOnce();
226 
227  // Wait until starting global localization
228  while (ros::ok())
229  {
230  rate.sleep();
231  ros::spinOnce();
232  if (status && status->status == mcl_3dl_msgs::Status::GLOBAL_LOCALIZATION)
233  break;
234  pub_cloud.publish(
235  generateCloudMsg(offset_x, offset_y, offset_z - laser_frame_height, offset_yaw));
236  pub_imu.publish(generateImuMsg());
237  pub_odom.publish(generateOdomMsg());
238  }
239  ASSERT_TRUE(ros::ok());
240 
241  // Wait until finishing global localization
242  while (ros::ok())
243  {
244  rate.sleep();
245  ros::spinOnce();
246  if (status && status->status == mcl_3dl_msgs::Status::NORMAL)
247  break;
248  pub_cloud.publish(
249  generateCloudMsg(offset_x, offset_y, offset_z - laser_frame_height, offset_yaw));
250  pub_imu.publish(generateImuMsg());
251  pub_odom.publish(generateOdomMsg());
252  }
253  ASSERT_TRUE(ros::ok());
254 
255  // Wait to improve accuracy
256  for (int i = 0; i < 40; ++i)
257  {
258  rate.sleep();
259  ros::spinOnce();
260  pub_cloud.publish(
261  generateCloudMsg(offset_x, offset_y, offset_z - laser_frame_height, offset_yaw));
262  pub_imu.publish(generateImuMsg());
263  pub_odom.publish(generateOdomMsg());
264  }
265  ASSERT_TRUE(ros::ok());
266 
267  ASSERT_TRUE(static_cast<bool>(status));
268  ASSERT_TRUE(static_cast<bool>(poses));
269 
270  const tf2::Transform true_pose(
271  tf2::Quaternion(0, 0, sinf(-offset_yaw / 2), cosf(-offset_yaw / 2)),
272  tf2::Vector3(
273  -(offset_x * cos(-offset_yaw) - offset_y * sin(-offset_yaw)),
274  -(offset_x * sin(-offset_yaw) + offset_y * cos(-offset_yaw)),
275  -offset_z));
276  bool found_true_positive(false);
277  for (const auto& pose : poses->poses)
278  {
279  tf2::Transform particle_pose;
280  tf2::fromMsg(pose, particle_pose);
281 
282  const tf2::Transform tf_diff = particle_pose.inverse() * true_pose;
283  if (tf_diff.getOrigin().length() < 2e-1 &&
284  fabs(tf2::getYaw(tf_diff.getRotation())) < 2e-1)
285  found_true_positive = true;
286  }
287  ASSERT_TRUE(found_true_positive);
288  }
289  }
290 }
291 
292 int main(int argc, char** argv)
293 {
294  testing::InitGoogleTest(&argc, argv);
295  ros::init(argc, argv, "test_global_localization");
296 
297  return RUN_ALL_TESTS();
298 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
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())
Quaternion getRotation() const
TEST(GlobalLocalization, Localize)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
TFSIMD_FORCE_INLINE const tfScalar & x() const
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & z() const
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
void fromMsg(const A &, B &b)
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
Transform inverse() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
double getYaw(const A &a)
static Time now()
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
ROSCPP_DECL void spinOnce()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Mon Jul 8 2019 03:32:36