test_mcl_3dl_compat.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-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 <cstdlib>
31 
32 #include <ros/ros.h>
33 #include <std_msgs/Bool.h>
34 #include <std_srvs/Empty.h>
35 
36 #include <gtest/gtest.h>
37 
38 #define UNDEF_COMPATIBILITY_LEVEL
39 
40 namespace mcl_3dl_compat
41 {
42 int current_level;
43 int supported_level;
44 int default_level;
45 } // namespace mcl_3dl_compat
47 
48 TEST(Mcl3DlCompat, ParamRename)
49 {
50  ros::NodeHandle nh;
51 
52  nh.setParam("param1", 1.0);
53  nh.setParam("param2", 2.0);
54  nh.setParam("param2_new", 3.0);
55  nh.setParam("param3_new", 4.0);
56 
57  mcl_3dl_compat::paramRename<double>(nh, "param1_new", "param1");
58  mcl_3dl_compat::paramRename<double>(nh, "param2_new", "param2");
59 
60  ASSERT_TRUE(nh.hasParam("param1_new"));
61  ASSERT_TRUE(nh.hasParam("param2_new"));
62  ASSERT_TRUE(nh.hasParam("param3_new"));
63 
64  double param1;
65  double param2;
66  double param3;
67  nh.getParam("param1_new", param1);
68  nh.getParam("param2_new", param2);
69  nh.getParam("param3_new", param3);
70 
71  // Only old parameter is provided for param1.
72  ASSERT_EQ(param1, 1.0);
73  // Both old and new paramters are provided for param2. New one must be active.
74  ASSERT_EQ(param2, 3.0);
75  // Only new paramter is provided for param3.
76  ASSERT_EQ(param3, 4.0);
77 }
78 
79 TEST(Mcl3DlCompat, CompatMode)
80 {
84 
85  ros::NodeHandle("~").setParam("compatible", 2);
86  ASSERT_NO_THROW(
87  {
89  }); // NOLINT(whitespace/braces)
90 
91  ros::NodeHandle("~").setParam("compatible", 3);
92  ASSERT_NO_THROW(
93  {
95  }); // NOLINT(whitespace/braces)
96 
97  ros::NodeHandle("~").setParam("compatible", 4);
98  ASSERT_THROW(
99  {
101  }, // NOLINT(whitespace/braces)
102  std::runtime_error);
103 
104  ros::NodeHandle("~").setParam("compatible", 1);
105  ASSERT_THROW(
106  {
108  }, // NOLINT(whitespace/braces)
109  std::runtime_error);
110 }
111 
113 {
114 public:
117  std_msgs::Bool::ConstPtr msg_;
118  mutable std_msgs::Bool::ConstPtr msg_const_;
120 
121  void cb(const std_msgs::Bool::ConstPtr& msg)
122  {
123  msg_ = msg;
124  }
125  void cbConst(const std_msgs::Bool::ConstPtr& msg) const
126  {
127  msg_const_ = msg;
128  }
129 
130  bool cbSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
131  {
132  srv_called_ = true;
133  return true;
134  }
135 
137  : pnh_("~")
138  , srv_called_(false)
139  {
140  }
141 };
142 
143 TEST(Mcl3DlCompat, Subscribe)
144 {
148 
150 
151  ros::Publisher pub_old = cls.pnh_.advertise<std_msgs::Bool>("test_old", 1, true);
152  ros::Publisher pub_new = cls.nh_.advertise<std_msgs::Bool>("test_new", 1, true);
153  std_msgs::Bool msg;
154  msg.data = false;
155  pub_old.publish(msg);
156  msg.data = true;
157  pub_new.publish(msg);
158 
159  {
160  cls.pnh_.setParam("compatible", 2);
162  cls.nh_, "test_new",
163  cls.pnh_, "test_old",
164  1,
166  ros::Duration(0.1).sleep();
167  ros::spinOnce();
168  ASSERT_TRUE(static_cast<bool>(cls.msg_));
169  ASSERT_EQ(false, static_cast<bool>(cls.msg_->data));
170  }
171 
172  {
173  cls.pnh_.setParam("compatible", 3);
174  cls.msg_ = nullptr;
176  cls.nh_, "test_new",
177  cls.pnh_, "test_old",
178  1,
180  ros::Duration(0.1).sleep();
181  ros::spinOnce();
182  ASSERT_TRUE(static_cast<bool>(cls.msg_));
183  ASSERT_EQ(true, static_cast<bool>(cls.msg_->data));
184  }
185 
186  {
187  cls.pnh_.setParam("compatible", 2);
188  cls.msg_ = nullptr;
190  cls.nh_, "test_new",
191  cls.pnh_, "test_old",
192  1,
194  ros::Duration(0.1).sleep();
195  ros::spinOnce();
196  ASSERT_TRUE(static_cast<bool>(cls.msg_const_));
197  ASSERT_EQ(false, static_cast<bool>(cls.msg_const_->data));
198  }
199 
200  {
201  cls.pnh_.setParam("compatible", 3);
202  cls.msg_ = nullptr;
204  cls.nh_, "test_new",
205  cls.pnh_, "test_old",
206  1,
208  ros::Duration(0.1).sleep();
209  ros::spinOnce();
210  ASSERT_TRUE(static_cast<bool>(cls.msg_const_));
211  ASSERT_EQ(true, static_cast<bool>(cls.msg_const_->data));
212  }
213 }
214 
215 TEST(Mcl3DlCompat, AdvertiseService)
216 {
220 
222  ros::ServiceClient cli_new = cls.nh_.serviceClient<std_srvs::Empty>("srv_new");
223  ros::ServiceClient cli_old = cls.pnh_.serviceClient<std_srvs::Empty>("srv_old");
224 
226  spinner.start();
227 
228  {
229  cls.pnh_.setParam("compatible", 2);
230 
232  cls.nh_, "srv_new",
233  cls.pnh_, "srv_old",
235  ros::Duration(0.1).sleep();
236  std_srvs::Empty empty;
237  ASSERT_TRUE(cli_old.call(empty.request, empty.response));
238  }
239 
240  {
241  cls.pnh_.setParam("compatible", 3);
242 
244  cls.nh_, "srv_new",
245  cls.pnh_, "srv_old",
247  ros::Duration(0.1).sleep();
248  std_srvs::Empty empty;
249  ASSERT_TRUE(cli_new.call(empty.request, empty.response));
250  }
251 
252  spinner.stop();
253 }
254 
255 int main(int argc, char** argv)
256 {
257  testing::InitGoogleTest(&argc, argv);
258  ros::init(argc, argv, "test_mcl_3dl_compat");
259 
260  return RUN_ALL_TESTS();
261 }
msg
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
const int default_level
Definition: compatibility.h:49
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cbConst(const std_msgs::Bool::ConstPtr &msg) const
bool call(MReq &req, MRes &res)
TEST(Mcl3DlCompat, ParamRename)
std_msgs::Bool::ConstPtr msg_const_
const int supported_level
Definition: compatibility.h:48
void spinner()
ros::ServiceServer advertiseService(ros::NodeHandle &nh_new, const std::string &service_new, ros::NodeHandle &nh_old, const std::string &service_old, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const int current_level
Definition: compatibility.h:47
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool cbSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
std_msgs::Bool::ConstPtr msg_
void checkCompatMode()
Definition: compatibility.h:62
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, void(T::*fp)(M) const, T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())
void cb(const std_msgs::Bool::ConstPtr &msg)
ROSCPP_DECL void spinOnce()
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const


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