test_expansion_resetting.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 <geometry_msgs/PoseWithCovarianceStamped.h>
34 #include <mcl_3dl_msgs/Status.h>
35 #include <nav_msgs/Odometry.h>
36 #include <sensor_msgs/Imu.h>
37 #include <sensor_msgs/PointCloud2.h>
39 #include <std_srvs/Trigger.h>
40 #include <tf2/utils.h>
42 
43 #include <random>
44 #include <vector>
45 
46 #include <gtest/gtest.h>
47 
48 namespace
49 {
50 void generateSamplePointcloud2(
51  sensor_msgs::PointCloud2& cloud,
52  const float offset_x,
53  const float offset_y,
54  const float offset_z)
55 {
56  cloud.height = 1;
57  cloud.is_bigendian = false;
58  cloud.is_dense = false;
59  sensor_msgs::PointCloud2Modifier modifier(cloud);
60  modifier.setPointCloud2Fields(
61  4,
62  "x", 1, sensor_msgs::PointField::FLOAT32,
63  "y", 1, sensor_msgs::PointField::FLOAT32,
64  "z", 1, sensor_msgs::PointField::FLOAT32,
65  "intensity", 1, sensor_msgs::PointField::FLOAT32);
66 
67  class Point
68  {
69  public:
70  float x_, y_, z_;
71  Point(const float x, const float y, const float z)
72  : x_(x)
73  , y_(y)
74  , z_(z)
75  {
76  }
77  };
78  std::vector<Point> points;
79  // Draw cube
80  for (float x = -1; x < 1; x += 0.05)
81  {
82  for (float y = -1; y < 1; y += 0.05)
83  {
84  points.push_back(Point(x / 2 + offset_x, y + offset_y, 1.0 + offset_z));
85  points.push_back(Point(x / 2 + offset_x, y + offset_y, -1.0 + offset_z));
86  points.push_back(Point(1.0 / 2 + offset_x, y + offset_y, x + offset_z));
87  points.push_back(Point(-1.0 / 2 + offset_x, y + offset_y, x + offset_z));
88  points.push_back(Point(x / 2 + offset_x, 1.0 + offset_y, y + offset_z));
89  points.push_back(Point(x / 2 + offset_x, -1.0 + offset_y, y + offset_z));
90  }
91  }
92 
93  modifier.resize(points.size());
94  cloud.width = points.size();
95  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
96  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
97  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
98 
99  for (const Point& p : points)
100  {
101  *iter_x = p.x_;
102  *iter_y = p.y_;
103  *iter_z = p.z_;
104  ++iter_x;
105  ++iter_y;
106  ++iter_z;
107  }
108 }
109 
110 inline sensor_msgs::PointCloud2 generateMapMsg(
111  const float offset_x,
112  const float offset_y,
113  const float offset_z)
114 {
115  sensor_msgs::PointCloud2 cloud;
116  generateSamplePointcloud2(cloud, offset_x, offset_y, offset_z);
117  cloud.header.frame_id = "map";
118  return cloud;
119 }
120 inline sensor_msgs::PointCloud2 generateCloudMsg()
121 {
122  sensor_msgs::PointCloud2 cloud;
123  generateSamplePointcloud2(cloud, 0, 0, 0);
124  cloud.header.frame_id = "base_link";
125  cloud.header.stamp = ros::Time::now();
126  return cloud;
127 }
128 inline sensor_msgs::Imu generateImuMsg()
129 {
130  sensor_msgs::Imu imu;
131  imu.header.frame_id = "base_link";
132  imu.header.stamp = ros::Time::now();
133  imu.orientation.w = 1;
134  imu.linear_acceleration.z = 9.8;
135  return imu;
136 }
137 inline nav_msgs::Odometry generateOdomMsg()
138 {
139  nav_msgs::Odometry odom;
140  odom.header.frame_id = "odom";
141  odom.header.stamp = ros::Time::now();
142  odom.pose.pose.position.x = 1;
143  odom.pose.pose.orientation.w = 1;
144  return odom;
145 }
146 inline geometry_msgs::PoseWithCovarianceStamped generateInitialPose()
147 {
148  geometry_msgs::PoseWithCovarianceStamped pose;
149  pose.header.frame_id = "map";
150  pose.header.stamp = ros::Time::now();
151  pose.pose.pose.orientation.w = 1.0;
152  pose.pose.covariance[6 * 0 + 0] = 0.05 * 0.05;
153  pose.pose.covariance[6 * 1 + 1] = 0.05 * 0.05;
154  pose.pose.covariance[6 * 2 + 2] = 0.05 * 0.05;
155  pose.pose.covariance[6 * 3 + 3] = 0.0;
156  pose.pose.covariance[6 * 4 + 4] = 0.0;
157  pose.pose.covariance[6 * 5 + 5] = 0.05 * 0.05;
158  return pose;
159 }
160 } // namespace
161 
162 class ExpansionResetting : public ::testing::Test
163 {
164 protected:
175 
176  geometry_msgs::PoseArray::ConstPtr poses_;
177  geometry_msgs::PoseWithCovarianceStamped::ConstPtr pose_cov_;
178  mcl_3dl_msgs::Status::ConstPtr status_;
179 
180  void cbPose(const geometry_msgs::PoseArray::ConstPtr& msg)
181  {
182  poses_ = msg;
183  }
184  void cbStatus(const mcl_3dl_msgs::Status::ConstPtr& msg)
185  {
186  status_ = msg;
187  }
188  void cbPoseCov(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
189  {
190  pose_cov_ = msg;
191  }
192  bool findTruePose(const tf2::Transform& true_pose)
193  {
194  if (!poses_)
195  return false;
196 
197  bool found_true_positive(false);
198  for (const auto& pose : poses_->poses)
199  {
200  tf2::Transform particle_pose;
201  tf2::fromMsg(pose, particle_pose);
202 
203  const tf2::Transform tf_diff = particle_pose.inverse() * true_pose;
204  if (tf_diff.getOrigin().length() < 2e-1 &&
205  fabs(tf2::getYaw(tf_diff.getRotation())) < 2e-1)
206  found_true_positive = true;
207  }
208  return found_true_positive;
209  }
210 
211  void SetUp()
212  {
213  ASSERT_TRUE(src_expansion_resetting_.waitForExistence(ros::Duration(10)));
214 
215  pub_init_.publish(generateInitialPose());
216  ros::Rate wait(10);
217  for (int i = 0; i < 100; i++)
218  {
219  wait.sleep();
220  ros::spinOnce();
221  if (pose_cov_)
222  break;
223  if (!ros::ok())
224  break;
225  }
226  }
227 
228 public:
230  {
231  sub_pose_ = nh_.subscribe("mcl_3dl/particles", 1, &ExpansionResetting::cbPose, this);
232  sub_status_ = nh_.subscribe("mcl_3dl/status", 1, &ExpansionResetting::cbStatus, this);
233  sub_pose_cov_ = nh_.subscribe("amcl_pose", 1, &ExpansionResetting::cbPoseCov, this);
234 
235  pub_mapcloud_ = nh_.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
236  pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud", 1);
237  pub_imu_ = nh_.advertise<sensor_msgs::Imu>("imu/data", 1);
238  pub_odom_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
239  pub_init_ =
240  nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, true);
241 
242  src_expansion_resetting_ =
243  nh_.serviceClient<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(
244  "expansion_resetting");
245  }
246 };
247 
248 TEST_F(ExpansionResetting, ExpandAndResume)
249 {
250  const float offset_x = 1;
251  const float offset_y = 0;
252  const float offset_z = 0;
253  pub_mapcloud_.publish(generateMapMsg(offset_x, offset_y, offset_z));
254 
255  ros::Duration(1.0).sleep();
256  ros::Rate rate(10);
257  // Wait until finishing expansion resetting
258  for (int i = 0; i < 40; ++i)
259  {
260  rate.sleep();
261  ros::spinOnce();
262  if (status_ && status_->status == mcl_3dl_msgs::Status::EXPANSION_RESETTING)
263  i = 0;
264  pub_cloud_.publish(generateCloudMsg());
265  pub_imu_.publish(generateImuMsg());
266  pub_odom_.publish(generateOdomMsg());
267  }
268  ASSERT_TRUE(ros::ok());
269 
270  ASSERT_TRUE(static_cast<bool>(status_));
271  ASSERT_TRUE(static_cast<bool>(poses_));
272 
273  ASSERT_TRUE(
274  findTruePose(tf2::Transform(
275  tf2::Quaternion(0, 0, 0, 1),
276  tf2::Vector3(offset_x, offset_y, offset_z))));
277 }
278 
280 {
281  const float offset_x = 1;
282  const float offset_y = 0;
283  const float offset_z = 0;
284  pub_mapcloud_.publish(generateMapMsg(offset_x, offset_y, offset_z));
285 
286  ASSERT_TRUE(ros::ok());
287  ros::Rate rate(10);
288 
289  // Ensure that the node is not in expansion resetting mode
290  for (int i = 0; i < 40; ++i)
291  {
292  rate.sleep();
293  ros::spinOnce();
294  if (i > 5 && status_ && status_->status != mcl_3dl_msgs::Status::EXPANSION_RESETTING)
295  break;
296 
297  pub_cloud_.publish(generateCloudMsg());
298  pub_imu_.publish(generateImuMsg());
299  pub_odom_.publish(generateOdomMsg());
300  }
301  ASSERT_TRUE(ros::ok());
302 
303  status_ = nullptr;
304  std_srvs::TriggerRequest req;
305  std_srvs::TriggerResponse res;
306  ASSERT_TRUE(src_expansion_resetting_.call(req, res));
307  ros::Duration(0.2).sleep();
308 
309  // Wait until finishing expansion resetting
310  for (int i = 0; i < 40; ++i)
311  {
312  rate.sleep();
313  ros::spinOnce();
314  if (status_)
315  {
316  ASSERT_NE(status_->status, mcl_3dl_msgs::Status::EXPANSION_RESETTING);
317  }
318  pub_cloud_.publish(generateCloudMsg());
319  pub_imu_.publish(generateImuMsg());
320  pub_odom_.publish(generateOdomMsg());
321  }
322  ASSERT_TRUE(ros::ok());
323 
324  ASSERT_TRUE(static_cast<bool>(status_));
325  ASSERT_TRUE(static_cast<bool>(poses_));
326 
327  ASSERT_TRUE(
328  findTruePose(tf2::Transform(
329  tf2::Quaternion(0, 0, 0, 1),
330  tf2::Vector3(offset_x, offset_y, offset_z))));
331 }
332 
333 int main(int argc, char** argv)
334 {
335  testing::InitGoogleTest(&argc, argv);
336  ros::init(argc, argv, "test_expansion_resetting");
337  ros::NodeHandle nh; // workaround to keep the test node during the process life time
338 
339  return RUN_ALL_TESTS();
340 }
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
bool sleep() const
mcl_3dl_msgs::Status::ConstPtr status_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cbPoseCov(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
TEST_F(ExpansionResetting, ExpandAndResume)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void cbPose(const geometry_msgs::PoseArray::ConstPtr &msg)
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
void fromMsg(const A &, B &b)
ROSCPP_DECL bool ok()
ros::ServiceClient src_expansion_resetting_
TFSIMD_FORCE_INLINE const tfScalar & y() const
Transform inverse() const
int main(int argc, char **argv)
bool findTruePose(const tf2::Transform &true_pose)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
double getYaw(const A &a)
void wait(int seconds)
void cbStatus(const mcl_3dl_msgs::Status::ConstPtr &msg)
static Time now()
geometry_msgs::PoseArray::ConstPtr poses_
ROSCPP_DECL void spinOnce()
geometry_msgs::PoseWithCovarianceStamped::ConstPtr pose_cov_


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