test_beam_label.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020, 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 <random>
31 #include <vector>
32 
33 #include <Eigen/Core>
34 
35 #include <ros/ros.h>
36 
37 #include <geometry_msgs/PoseWithCovarianceStamped.h>
38 #include <nav_msgs/Odometry.h>
39 #include <sensor_msgs/Imu.h>
40 #include <sensor_msgs/PointCloud2.h>
42 #include <tf2/utils.h>
44 
45 #include <gtest/gtest.h>
46 
47 namespace
48 {
49 void generateSamplePointcloud2(
50  sensor_msgs::PointCloud2& cloud,
51  const float offset_x,
52  const float offset_y,
53  const float offset_z,
54  const float range)
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  "label", 1, sensor_msgs::PointField::UINT32);
66 
67  const float resolution = 0.05;
68 
69  std::vector<Eigen::Vector4d> points;
70  // Floor
71  for (float x = -range; x < range; x += resolution)
72  {
73  for (float y = -range; y < range; y += resolution)
74  {
75  points.emplace_back(x, y, -1.0, 0);
76  }
77  }
78  // Semi-transparent wall
79  for (float x = -0.5; x < 0.5; x += resolution)
80  {
81  for (float z = -1.0; z < 0.0; z += resolution)
82  {
83  points.emplace_back(x, 1.0, z, 2);
84  }
85  }
86 
87  modifier.resize(points.size());
88  cloud.width = points.size();
89  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
90  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
91  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
92  sensor_msgs::PointCloud2Iterator<uint32_t> iter_label(cloud, "label");
93 
94  for (const Eigen::Vector4d& p : points)
95  {
96  *iter_x = p[0] + offset_x;
97  *iter_y = p[1] + offset_y;
98  *iter_z = p[2] + offset_z;
99  *iter_label = static_cast<uint32_t>(p[3]);
100  ++iter_x;
101  ++iter_y;
102  ++iter_z;
103  ++iter_label;
104  }
105 }
106 
107 inline sensor_msgs::PointCloud2 generateMapMsg(
108  const float offset_x,
109  const float offset_y,
110  const float offset_z)
111 {
112  sensor_msgs::PointCloud2 cloud;
113  generateSamplePointcloud2(cloud, offset_x, offset_y, offset_z, 5.0);
114  cloud.header.frame_id = "map";
115  return cloud;
116 }
117 inline sensor_msgs::PointCloud2 generateCloudMsg()
118 {
119  sensor_msgs::PointCloud2 cloud;
120  generateSamplePointcloud2(cloud, 0, 0, 0, 2.0);
121  cloud.header.frame_id = "base_link";
122  cloud.header.stamp = ros::Time::now();
123  return cloud;
124 }
125 inline sensor_msgs::Imu generateImuMsg()
126 {
127  sensor_msgs::Imu imu;
128  imu.header.frame_id = "base_link";
129  imu.header.stamp = ros::Time::now();
130  imu.orientation.w = 1;
131  imu.linear_acceleration.z = 9.8;
132  return imu;
133 }
134 inline nav_msgs::Odometry generateOdomMsg()
135 {
136  nav_msgs::Odometry odom;
137  odom.header.frame_id = "odom";
138  odom.header.stamp = ros::Time::now();
139  odom.pose.pose.position.x = 1;
140  odom.pose.pose.orientation.w = 1;
141  return odom;
142 }
143 inline geometry_msgs::PoseWithCovarianceStamped generateInitialPose()
144 {
145  geometry_msgs::PoseWithCovarianceStamped pose;
146  pose.header.frame_id = "map";
147  pose.header.stamp = ros::Time::now();
148  pose.pose.pose.orientation.w = 1.0;
149  pose.pose.covariance[6 * 0 + 0] = std::pow(0.2, 2);
150  pose.pose.covariance[6 * 1 + 1] = std::pow(0.2, 2);
151  pose.pose.covariance[6 * 2 + 2] = std::pow(0.2, 2);
152  pose.pose.covariance[6 * 3 + 3] = 0.0;
153  pose.pose.covariance[6 * 4 + 4] = 0.0;
154  pose.pose.covariance[6 * 5 + 5] = std::pow(0.05, 2);
155  return pose;
156 }
157 } // namespace
158 
159 class BeamLabel : public ::testing::Test
160 {
161 protected:
169 
170  geometry_msgs::PoseWithCovarianceStamped::ConstPtr pose_cov_;
171 
172  void cbPoseCov(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
173  {
174  pose_cov_ = msg;
175  }
176 
177  void SetUp()
178  {
179  pub_init_.publish(generateInitialPose());
180  ros::Rate wait(10);
181  for (int i = 0; i < 100; i++)
182  {
183  wait.sleep();
184  ros::spinOnce();
185  if (pose_cov_)
186  break;
187  if (!ros::ok())
188  break;
189  }
190  }
191 
192 public:
194  {
195  sub_pose_cov_ = nh_.subscribe("amcl_pose", 1, &BeamLabel::cbPoseCov, this);
196 
197  pub_mapcloud_ = nh_.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
198  pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud", 1);
199  pub_imu_ = nh_.advertise<sensor_msgs::Imu>("imu/data", 1);
200  pub_odom_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
201  pub_init_ =
202  nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, true);
203  }
204 };
205 
206 TEST_F(BeamLabel, SemiTransparentWall)
207 {
208  const float offset_x = 0.05;
209  const float offset_y = 0.05;
210  const float offset_z = 0;
211  pub_mapcloud_.publish(generateMapMsg(offset_x, offset_y, offset_z));
212 
213  ros::Duration(1.0).sleep();
214  ros::Rate rate(10);
215  for (int i = 0; i < 100; ++i)
216  {
217  rate.sleep();
218  ros::spinOnce();
219  pub_cloud_.publish(generateCloudMsg());
220  pub_imu_.publish(generateImuMsg());
221  pub_odom_.publish(generateOdomMsg());
222  }
223  ASSERT_TRUE(ros::ok());
224 
225  ASSERT_TRUE(static_cast<bool>(pose_cov_));
226 
227  ASSERT_NEAR(pose_cov_->pose.pose.position.x, offset_x, 0.1);
228  ASSERT_NEAR(pose_cov_->pose.pose.position.y, offset_y, 0.1);
229  ASSERT_NEAR(pose_cov_->pose.pose.position.z, offset_z, 0.1);
230 }
231 
232 int main(int argc, char** argv)
233 {
234  testing::InitGoogleTest(&argc, argv);
235  ros::init(argc, argv, "test_beam_label");
236  ros::NodeHandle nh; // workaround to keep the test node during the process life time
237 
238  return RUN_ALL_TESTS();
239 }
ros::Publisher pub_imu_
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_odom_
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)
ros::NodeHandle nh_
TFSIMD_FORCE_INLINE const tfScalar & y() const
TEST_F(BeamLabel, SemiTransparentWall)
ros::Publisher pub_cloud_
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool sleep()
void wait(int seconds)
ros::Publisher pub_mapcloud_
static Time now()
geometry_msgs::PoseWithCovarianceStamped::ConstPtr pose_cov_
ros::Publisher pub_init_
void cbPoseCov(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
ROSCPP_DECL void spinOnce()
ros::Subscriber sub_pose_cov_


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