test_dynamicReconfigure.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <ros/ros.h>
3 #include <dynamic_reconfigure/server.h>
4 
5 #include <rosparam_handler/DefaultsParameters.h>
6 
7 typedef rosparam_handler::DefaultsParameters ParamType;
8 typedef rosparam_handler::DefaultsConfig ConfigType;
9 
10 class TestDynamicReconfigure : public testing::Test
11 {
12 public:
13 
15  : nh("~")
16  , testParams(nh)
17  , drSrv(nh)
18  {
19  auto cb = boost::bind(&TestDynamicReconfigure::configCallback, this, _1, _2);
20  drSrv.setCallback(cb);
21  }
22 
23  ~TestDynamicReconfigure() = default;
24 
25  void configCallback(ConfigType& config, uint32_t /*level*/)
26  {
27  testParams.fromConfig(config);
28  }
29 
31  {
32  testParams.toConfig(config);
33  }
34 
36 
38 
39  dynamic_reconfigure::Server<ConfigType> drSrv;
40 };
41 
43 {
44  // Delete in case they are still on the server.
45  nh.deleteParam("int_param_w_default");
46  nh.deleteParam("double_param_w_default");
47  nh.deleteParam("str_param_w_default");
48  nh.deleteParam("bool_param_w_default");
49 
50  ASSERT_NO_THROW(testParams.fromParamServer());
51 
52  ASSERT_EQ(1, testParams.int_param_w_default);
53  ASSERT_DOUBLE_EQ(1.1, testParams.double_param_w_default);
54  ASSERT_EQ("Hello World", testParams.str_param_w_default);
55  ASSERT_TRUE(testParams.bool_param_w_default);
56 
57  // ASSERT_EQ(std::vector<int>({1, 2, 3}), testParams.vector_int_param_w_default);
58  // ASSERT_EQ(std::vector<double>({1.1, 1.2, 1.3}), testParams.vector_double_param_w_default);
59  // ASSERT_EQ(std::vector<bool>({false, true}), testParams.vector_bool_param_w_default);
60  // ASSERT_EQ(std::vector<std::string>({"Hello", "World"}), testParams.vector_string_param_w_default);
61 
62  // std::map<std::string, std::string> tmp{{"Hello", "World"}};
63  // ASSERT_EQ(tmp, testParams.map_param_w_default);
64 
65  // ASSERT_EQ(1, testParams.enum_param_w_default);
66 
67  dynamic_reconfigure::ReconfigureRequest srv_req;
68  dynamic_reconfigure::ReconfigureResponse srv_resp;
69  dynamic_reconfigure::Config conf;
70 
71  dynamic_reconfigure::IntParameter int_param;
72  int_param.name = "int_param_w_default";
73  int_param.value = 2;
74 
75  dynamic_reconfigure::DoubleParameter double_param;
76  double_param.name = "double_param_w_default";
77  double_param.value = 2.2;
78 
79  dynamic_reconfigure::StrParameter str_param;
80  str_param.name = "str_param_w_default";
81  str_param.value = "Foo Bar";
82 
83  dynamic_reconfigure::BoolParameter bool_param;
84  bool_param.name = "bool_param_w_default";
85  bool_param.value = false;
86 
87  conf.ints.push_back(int_param);
88  conf.doubles.push_back(double_param);
89  conf.strs.push_back(str_param);
90  conf.bools.push_back(bool_param);
91 
92  srv_req.config = conf;
93 
94  ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv_req, srv_resp));
95 
96  ros::Duration(1).sleep();
97 
98  EXPECT_EQ(2, testParams.int_param_w_default);
99  EXPECT_DOUBLE_EQ(2.2, testParams.double_param_w_default);
100  EXPECT_EQ("Foo Bar", testParams.str_param_w_default);
101  EXPECT_FALSE(testParams.bool_param_w_default);
102 }
103 
105 {
106  ConfigType config;
107 
108  rosparamToConfig(config);
109 
110  EXPECT_EQ(testParams.int_param_w_default, config.int_param_w_default);
111  EXPECT_DOUBLE_EQ(testParams.double_param_w_default, config.double_param_w_default);
112  EXPECT_EQ(testParams.str_param_w_default, config.str_param_w_default);
113  EXPECT_FALSE(config.bool_param_w_default);
114 }
dynamic_reconfigure::Server< ConfigType > drSrv
TEST_F(TestDynamicReconfigure, DynamicReconf)
bool deleteParam(const std::string &key) const
rosparam_handler::DefaultsParameters ParamType
bool call(const std::string &service_name, MReq &req, MRes &res)
bool sleep() const
~TestDynamicReconfigure()=default
rosparam_handler::DefaultsConfig ConfigType
void configCallback(ConfigType &config, uint32_t)
const std::string & getNamespace() const
rosparam_handler::DefaultsConfig ConfigType
void rosparamToConfig(ConfigType &config)
rosparam_handler::DefaultsParameters ParamType


rosparam_handler
Author(s): Claudio Bandera
autogenerated on Wed Jun 5 2019 19:50:36