test_transform_failure.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 <ros/master.h>
33 #include <geometry_msgs/TransformStamped.h>
35 #include <sensor_msgs/PointCloud2.h>
38 
39 #include <string>
40 
41 #include <gtest/gtest.h>
42 
43 namespace
44 {
45 void GenerateSinglePointPointcloud2(
46  sensor_msgs::PointCloud2& cloud,
47  const float x, const float y, const float z)
48 {
49  cloud.height = 1;
50  cloud.width = 1;
51  cloud.is_bigendian = false;
52  cloud.is_dense = false;
53  sensor_msgs::PointCloud2Modifier modifier(cloud);
54  modifier.setPointCloud2FieldsByString(1, "xyz");
55  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
56  sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
57  sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
58  modifier.resize(1);
59  *iter_x = x;
60  *iter_y = y;
61  *iter_z = z;
62 }
63 void publishSinglePointPointcloud2(
64  ros::Publisher& pub,
65  const float x, const float y, const float z,
66  const std::string frame_id,
67  const ros::Time stamp)
68 {
69  sensor_msgs::PointCloud2 cloud;
70  cloud.header.frame_id = frame_id;
71  cloud.header.stamp = stamp;
72  GenerateSinglePointPointcloud2(cloud, x, y, z);
73  pub.publish(cloud);
74 }
75 } // namespace
76 
77 TEST(TransformFailure, NoDeadAgainstTransformFailure)
78 {
79  ros::NodeHandle nh("");
81  ros::Publisher pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
82  ros::Publisher pub_mapcloud = nh.advertise<sensor_msgs::PointCloud2>("mapcloud", 1, true);
83 
84  ros::Rate rate(10);
85  publishSinglePointPointcloud2(
86  pub_mapcloud,
87  0.0, 0.0, 0.0, "map", ros::Time::now());
88 
89  while (ros::ok())
90  {
91  ros::spinOnce();
92  rate.sleep();
93 
94  ros::V_string nodes;
95  ros::master::getNodes(nodes);
96  bool found = false;
97  for (auto node : nodes)
98  {
99  if (node == "/mcl_3dl")
100  {
101  found = true;
102  break;
103  }
104  }
105  if (found)
106  break;
107  }
108  std::cerr << "mcl_3dl started" << std::endl;
109  ros::Duration(1.0).sleep();
110  int cnt = 0;
111  // mcl_3dl is launched with required="true". This test is killed by roslaunch if mcl_3dl is dead.
112  while (ros::ok())
113  {
114  ++cnt;
115  geometry_msgs::TransformStamped trans;
116  trans.header.stamp = ros::Time::now() + ros::Duration(0.1);
117  trans.transform.rotation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1));
118 
119  trans.header.frame_id = "laser_link_base";
120  trans.child_frame_id = "laser_link";
121  tfb.sendTransform(trans);
122 
123  trans.header.frame_id = "map";
124  trans.child_frame_id = "odom";
125  tfb.sendTransform(trans);
126 
127  if (cnt > 10)
128  {
129  trans.header.frame_id = "base_link";
130  trans.child_frame_id = "laser_link_base";
131  tfb.sendTransform(trans);
132  }
133  if (cnt > 20)
134  {
135  trans.header.frame_id = "odom";
136  trans.child_frame_id = "base_link";
137  tfb.sendTransform(trans);
138  }
139  if (cnt > 30)
140  break;
141 
142  publishSinglePointPointcloud2(
143  pub_cloud,
144  0.0, 0.0, 0.0, "laser_link", ros::Time::now());
145 
146  ros::spinOnce();
147  rate.sleep();
148  }
149  ASSERT_TRUE(ros::ok());
150 }
151 
152 int main(int argc, char** argv)
153 {
154  testing::InitGoogleTest(&argc, argv);
155  ros::init(argc, argv, "test_transform_failure");
156 
157  return RUN_ALL_TESTS();
158 }
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST(TransformFailure, NoDeadAgainstTransformFailure)
std::vector< std::string > V_string
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void sendTransform(const geometry_msgs::TransformStamped &transform)
B toMsg(const A &a)
static Time now()
ROSCPP_DECL bool getNodes(V_string &nodes)
ROSCPP_DECL void spinOnce()


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