diff_drive_dyn_reconf_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #include "test_common.h"
31 #include <tf/transform_listener.h>
32 
33 #include <dynamic_reconfigure/server.h>
34 
35 // TEST CASES
36 TEST_F(DiffDriveControllerTest, testDynReconfServerAlive)
37 {
38  // wait for ROS
39  while(!isControllerAlive() && ros::ok())
40  {
41  ros::Duration(0.1).sleep();
42  }
43  if (!ros::ok())
44  FAIL() << "Something went wrong while executing test";
45 
46  // Expect server is alive
47  EXPECT_TRUE(ros::service::exists("diffbot_controller/set_parameters", true));
48 
49  dynamic_reconfigure::ReconfigureRequest srv_req;
50  dynamic_reconfigure::ReconfigureResponse srv_resp;
51 
52  // Expect server is callable (get-fashion)
53  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
54 
55  EXPECT_EQ(1, srv_resp.config.bools.size());
56 
57  if (!srv_resp.config.bools.empty())
58  {
59  EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
60  // expect false since it is set to false in the .test
61  EXPECT_EQ(false, srv_resp.config.bools[0].value);
62  }
63 
64  EXPECT_EQ(4, srv_resp.config.doubles.size());
65 
66  if (srv_resp.config.doubles.size() >= 4)
67  {
68  EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
69  EXPECT_EQ(1, srv_resp.config.doubles[0].value);
70 
71  EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
72  EXPECT_EQ(1, srv_resp.config.doubles[1].value);
73 
74  EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
75  EXPECT_EQ(1, srv_resp.config.doubles[2].value);
76 
77  EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
78  EXPECT_EQ(50, srv_resp.config.doubles[3].value);
79  }
80 
81  dynamic_reconfigure::DoubleParameter double_param;
82  double_param.name = "left_wheel_radius_multiplier";
83  double_param.value = 0.95;
84 
85  srv_req.config.doubles.push_back(double_param);
86 
87  double_param.name = "right_wheel_radius_multiplier";
88  double_param.value = 0.95;
89 
90  srv_req.config.doubles.push_back(double_param);
91 
92  double_param.name = "wheel_separation_multiplier";
93  double_param.value = 0.95;
94 
95  srv_req.config.doubles.push_back(double_param);
96 
97  double_param.name = "publish_rate";
98  double_param.value = 150;
99 
100  srv_req.config.doubles.push_back(double_param);
101 
102  dynamic_reconfigure::BoolParameter bool_param;
103  bool_param.name = "enable_odom_tf";
104  bool_param.value = false;
105 
106  srv_req.config.bools.push_back(bool_param);
107 
108  // Expect server is callable (set-fashion)
109  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
110 
111  EXPECT_EQ(1, srv_resp.config.bools.size());
112 
113  if (!srv_resp.config.bools.empty())
114  {
115  EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
116  EXPECT_EQ(false, srv_resp.config.bools[0].value);
117  }
118 
119  EXPECT_EQ(4, srv_resp.config.doubles.size());
120 
121  if (srv_resp.config.doubles.size() >= 4)
122  {
123  EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
124  EXPECT_EQ(0.95, srv_resp.config.doubles[0].value);
125 
126  EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
127  EXPECT_EQ(0.95, srv_resp.config.doubles[1].value);
128 
129  EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
130  EXPECT_EQ(0.95, srv_resp.config.doubles[2].value);
131 
132  EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
133  EXPECT_EQ(150, srv_resp.config.doubles[3].value);
134  }
135 }
136 
137 // TEST CASES
138 TEST_F(DiffDriveControllerTest, testDynReconfEnableTf)
139 {
140  // wait for ROS
141  while(!isControllerAlive() && ros::ok())
142  {
143  ros::Duration(0.1).sleep();
144  }
145  if (!ros::ok())
146  FAIL() << "Something went wrong while executing test";
147 
148  // set up tf listener
149  tf::TransformListener listener;
150  ros::Duration(2.0).sleep();
151  // check the odom frame doesn't exist
152  EXPECT_FALSE(listener.frameExists("odom"));
153 
154  dynamic_reconfigure::ReconfigureRequest srv_req;
155  dynamic_reconfigure::ReconfigureResponse srv_resp;
156 
157  dynamic_reconfigure::BoolParameter bool_param;
158  bool_param.name = "enable_odom_tf";
159  bool_param.value = true;
160 
161  srv_req.config.bools.push_back(bool_param);
162 
163  EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
164 
165  ros::Duration(2.0).sleep();
166  // check the odom frame doesn't exist
167  EXPECT_TRUE(listener.frameExists("odom"));
168 }
169 
170 int main(int argc, char** argv)
171 {
172  testing::InitGoogleTest(&argc, argv);
173  ros::init(argc, argv, "diff_drive_dyn_reconf_test");
174 
176  spinner.start();
177  int ret = RUN_ALL_TESTS();
178  spinner.stop();
179  ros::shutdown();
180  return ret;
181 }
bool call(const std::string &service_name, MReq &req, MRes &res)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
ROSCPP_DECL bool ok()
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
TEST_F(DiffDriveControllerTest, testDynReconfServerAlive)
bool frameExists(const std::string &frame_id_str) const


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05