Skip to content

Commit 37b8777

Browse files
mikaelarguedasmikaelarguedas
authored
Reconfigure client cpp (#78)
* Added C++ Client class * typo * use configuration server rather than ActionServer * clearer Debug message * add check for values in getConfigs * trying longer timeouts, it may help getting correct configs * trailing whitespace
1 parent 492f94a commit 37b8777

File tree

5 files changed

+478
-3
lines changed

5 files changed

+478
-3
lines changed

‎CMakeLists.txt

+3-3
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,15 @@ cmake_minimum_required(VERSION 2.8)
22
project(dynamic_reconfigure)
33

44
find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs)
5-
find_package(Boost REQUIRED COMPONENTS system thread)
5+
find_package(Boost REQUIRED COMPONENTS system thread chrono)
66

77
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
88

99
catkin_python_setup()
1010

1111
add_message_files(
1212
DIRECTORY msg
13-
FILES
13+
FILES
1414
BoolParameter.msg Config.msg Group.msg IntParameter.msg SensorLevels.msg
1515
ConfigDescription.msg DoubleParameter.msg GroupState.msg ParamDescription.msg StrParameter.msg)
1616

@@ -41,7 +41,7 @@ target_link_libraries(dynamic_reconfigure_config_init_mutex ${Boost_LIBRARIES})
4141
install(DIRECTORY include/dynamic_reconfigure/
4242
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
4343
FILES_MATCHING PATTERN "*.h")
44-
44+
4545
install(TARGETS dynamic_reconfigure_config_init_mutex
4646
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
4747
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

‎include/dynamic_reconfigure/client.h

+342
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,342 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2015-2016, Myrmex, Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of the Myrmex, Inc nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*********************************************************************/
34+
35+
/**
36+
37+
Author: Aris Synodinos
38+
39+
Handles sychronizing node state with the configuration server and setting/getting
40+
configuration.
41+
42+
*/
43+
44+
#ifndef __CLIENT_H__
45+
#define __CLIENT_H__
46+
47+
#include <boost/chrono.hpp>
48+
#include <boost/function.hpp>
49+
#include <boost/thread.hpp>
50+
#include <ros/node_handle.h>
51+
#include <dynamic_reconfigure/ConfigDescription.h>
52+
#include <dynamic_reconfigure/Reconfigure.h>
53+
54+
namespace dynamic_reconfigure {
55+
56+
template <class ConfigType>
57+
class Client {
58+
public:
59+
/**
60+
* @brief Client Constructs a statefull dynamic_reconfigure client
61+
* @param name The full path of the dynamic_reconfigure::Server
62+
* @param config_callback A callback that should be called when the server
63+
* informs the clients of a successful reconfiguration
64+
* @param description_callback A callback that should be called when the
65+
* server infrorms the clients of the description of the reconfiguration
66+
* parameters and groups
67+
*/
68+
Client(
69+
const std::string& name,
70+
const boost::function<void(const ConfigType&)> config_callback = 0,
71+
const boost::function<void(const dynamic_reconfigure::ConfigDescription&)>
72+
description_callback = 0)
73+
: name_(name),
74+
nh_(name),
75+
received_configuration_(false),
76+
received_description_(false),
77+
config_callback_(config_callback),
78+
description_callback_(description_callback) {
79+
set_service_ =
80+
nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
81+
82+
config_sub_ =
83+
nh_.subscribe("parameter_updates", 1,
84+
&Client<ConfigType>::configurationCallback, this);
85+
86+
descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
87+
&Client<ConfigType>::descriptionCallback, this);
88+
}
89+
/**
90+
* @brief Client Constructs a statefull dynamic_reconfigure client
91+
* @param name The full path of the dynamic_reconfigure::Server
92+
* @param nh The nodehandle to the full path of the Server (for nodelets)
93+
* @param config_callback A callback that should be called when the server
94+
* informs the clients of a successful reconfiguration
95+
* @param description_callback A callback that should be called when the
96+
* server infrorms the clients of the description of the reconfiguration
97+
* parameters and groups
98+
*/
99+
Client(
100+
const std::string& name, const ros::NodeHandle& nh,
101+
const boost::function<void(const ConfigType&)> config_callback = 0,
102+
const boost::function<void(const dynamic_reconfigure::ConfigDescription&)>
103+
description_callback = 0)
104+
: name_(name),
105+
nh_(nh),
106+
received_configuration_(false),
107+
received_description_(false),
108+
config_callback_(config_callback),
109+
description_callback_(description_callback) {
110+
set_service_ =
111+
nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
112+
113+
config_sub_ =
114+
nh_.subscribe("parameter_updates", 1,
115+
&Client<ConfigType>::configurationCallback, this);
116+
117+
descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
118+
&Client<ConfigType>::descriptionCallback, this);
119+
}
120+
/**
121+
* @brief setConfigurationCallback Sets the user defined configuration
122+
* callback function
123+
* @param config_callback A function pointer
124+
*/
125+
void setConfigurationCallback(
126+
const boost::function<void(const ConfigType&)>& config_callback) {
127+
config_callback_ = config_callback;
128+
}
129+
/**
130+
* @brief setDescriptionCallback Sets the user defined description callback
131+
* function
132+
* @param description_callback A function pointer
133+
*/
134+
void setDescriptionCallback(const boost::function<void(
135+
const dynamic_reconfigure::ConfigDescription&)>& description_callback) {
136+
description_callback_ = description_callback;
137+
}
138+
/**
139+
* @brief setConfiguration Attempts to set the configuration to the server
140+
* @param configuration The requested configuration
141+
* @return True if the server accepted the request (not the reconfiguration)
142+
*/
143+
bool setConfiguration(const ConfigType& configuration) {
144+
ConfigType temp = configuration;
145+
return setConfiguration(temp);
146+
}
147+
/**
148+
* @brief setConfiguration Attempts to set the configuration to the server
149+
* @param configuration The requested configuration, gets overwritten with the
150+
* reply from the reconfigure server
151+
* @return True if the server accepted the request (not the reconfiguration)
152+
*/
153+
bool setConfiguration(ConfigType& configuration) {
154+
dynamic_reconfigure::Reconfigure srv;
155+
configuration.__toMessage__(srv.request.config);
156+
if (set_service_.call(srv)) {
157+
configuration.__fromMessage__(srv.response.config);
158+
return true;
159+
} else {
160+
ROS_WARN("Could not set configuration");
161+
return false;
162+
}
163+
}
164+
/**
165+
* @brief getCurrentConfiguration Gets the latest configuration from the
166+
* dynamic_reconfigure::Server
167+
* @param configuration The object where the configuration will be stored
168+
* @param timeout The duration that the client should wait for the
169+
* configuration, if set to ros::Duration(0) will wait indefinetely
170+
* @return False if the timeout has happened
171+
*/
172+
bool getCurrentConfiguration(
173+
ConfigType& configuration,
174+
const ros::Duration& timeout = ros::Duration(0)) {
175+
if (timeout == ros::Duration(0)) {
176+
ROS_INFO_ONCE("Waiting for configuration...");
177+
boost::mutex::scoped_lock lock(mutex_);
178+
while (!received_configuration_) {
179+
if (!ros::ok()) return false;
180+
cv_.wait(lock);
181+
}
182+
} else {
183+
ros::Time start_time = ros::Time::now();
184+
boost::mutex::scoped_lock lock(mutex_);
185+
while (!received_configuration_) {
186+
if (!ros::ok()) return false;
187+
ros::Duration time_left = timeout - (ros::Time::now() - start_time);
188+
if (time_left.toSec() <= 0.0) return false;
189+
cv_.wait_for(lock, boost::chrono::nanoseconds(time_left.toNSec()));
190+
}
191+
}
192+
configuration = latest_configuration_;
193+
return true;
194+
}
195+
/**
196+
* @brief getDefaultConfiguration Gets the latest default configuration from
197+
* the dynamic_reconfigure::Server
198+
* @param configuration The object where the configuration will be stored
199+
* @param timeout The duration that the client should wait for the
200+
* configuration, if set to ros::Duration(0) will wait indefinetely
201+
* @return False if the timeout has happened
202+
*/
203+
bool getDefaultConfiguration(
204+
ConfigType& configuration,
205+
const ros::Duration& timeout = ros::Duration(0)) {
206+
ConfigDescription answer;
207+
if (getDescription(answer, timeout)) {
208+
configuration.__fromMessage__(answer.dflt);
209+
return true;
210+
} else {
211+
return false;
212+
}
213+
}
214+
/**
215+
* @brief getMinConfiguration Gets the latest minimum configuration from
216+
* the dynamic_reconfigure::Server
217+
* @param configuration The object where the configuration will be stored
218+
* @param timeout The duration that the client should wait for the
219+
* configuration, if set to ros::Duration(0) will wait indefinetely
220+
* @return False if the timeout has happened
221+
*/
222+
bool getMinConfiguration(ConfigType& configuration,
223+
const ros::Duration& timeout = ros::Duration(0)) {
224+
ConfigDescription answer;
225+
if (getDescription(answer, timeout)) {
226+
configuration.__fromMessage__(answer.min);
227+
return true;
228+
} else {
229+
return false;
230+
}
231+
}
232+
/**
233+
* @brief getMaxConfiguration Gets the latest maximum configuration from
234+
* the dynamic_reconfigure::Server
235+
* @param configuration The object where the configuration will be stored
236+
* @param timeout The duration that the client should wait for the
237+
* configuration, if set to ros::Duration(0) will wait indefinetely
238+
* @return False if the timeout has happened
239+
*/
240+
bool getMaxConfiguration(ConfigType& configuration,
241+
const ros::Duration& timeout = ros::Duration(0)) {
242+
ConfigDescription answer;
243+
if (getDescription(answer, timeout)) {
244+
configuration.__fromMessage__(answer.max);
245+
return true;
246+
} else {
247+
return false;
248+
}
249+
}
250+
/**
251+
* @brief getName Gets the name of the Dynamic Reconfigure Client
252+
* @return Copy of the member variable
253+
*/
254+
std::string getName() const { return name_; }
255+
256+
private:
257+
void configurationCallback(const dynamic_reconfigure::Config& configuration) {
258+
boost::mutex::scoped_lock lock(mutex_);
259+
dynamic_reconfigure::Config temp_config = configuration;
260+
received_configuration_ = true;
261+
latest_configuration_.__fromMessage__(temp_config);
262+
cv_.notify_all();
263+
264+
if (config_callback_) {
265+
try {
266+
config_callback_(latest_configuration_);
267+
} catch (std::exception& e) {
268+
ROS_WARN("Configuration callback failed with exception %s", e.what());
269+
} catch (...) {
270+
ROS_WARN("Configuration callback failed with unprintable exception");
271+
}
272+
} else {
273+
ROS_DEBUG(
274+
"Unable to call Configuration callback because none was set.\n" \
275+
"See setConfigurationCallback");
276+
}
277+
}
278+
279+
void descriptionCallback(
280+
const dynamic_reconfigure::ConfigDescription& description) {
281+
boost::mutex::scoped_lock lock(mutex_);
282+
received_description_ = true;
283+
latest_description_ = description;
284+
cv_.notify_all();
285+
286+
if (description_callback_) {
287+
try {
288+
description_callback_(description);
289+
} catch (std::exception& e) {
290+
ROS_WARN("Description callback failed with exception %s", e.what());
291+
} catch (...) {
292+
ROS_WARN("Description callback failed with unprintable exception");
293+
}
294+
} else {
295+
ROS_DEBUG(
296+
"Unable to call Description callback because none was set.\n" \
297+
"See setDescriptionCallback");
298+
}
299+
}
300+
301+
bool getDescription(ConfigDescription& configuration,
302+
const ros::Duration& timeout) {
303+
if (timeout == ros::Duration(0)) {
304+
ROS_INFO_ONCE("Waiting for configuration...");
305+
boost::mutex::scoped_lock lock(mutex_);
306+
while (!received_description_) {
307+
if (!ros::ok()) return false;
308+
cv_.wait(lock);
309+
}
310+
} else {
311+
ros::Time start_time = ros::Time::now();
312+
boost::mutex::scoped_lock lock(mutex_);
313+
while (!received_description_) {
314+
if (!ros::ok()) return false;
315+
ros::Duration time_left = timeout - (ros::Time::now() - start_time);
316+
if (time_left.toSec() <= 0.0) return false;
317+
cv_.wait_for(lock, boost::chrono::nanoseconds(time_left.toNSec()));
318+
}
319+
}
320+
configuration = latest_description_;
321+
return true;
322+
}
323+
324+
std::string name_;
325+
bool received_configuration_;
326+
ConfigType latest_configuration_;
327+
bool received_description_;
328+
dynamic_reconfigure::ConfigDescription latest_description_;
329+
boost::condition_variable cv_;
330+
boost::mutex mutex_;
331+
ros::NodeHandle nh_;
332+
ros::ServiceClient set_service_;
333+
ros::Subscriber descr_sub_;
334+
ros::Subscriber config_sub_;
335+
336+
boost::function<void(const ConfigType&)> config_callback_;
337+
boost::function<void(const dynamic_reconfigure::ConfigDescription&)>
338+
description_callback_;
339+
};
340+
}
341+
342+
#endif // __CLIENT_H__

‎test/CMakeLists.txt

+6
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,10 @@ target_link_libraries(dynamic_reconfigure-ref_server pthread dynamic_reconfigure
44

55
add_dependencies(tests dynamic_reconfigure-ref_server)
66

7+
add_rostest_gtest(dynamic_reconfigure-test_client test_cpp_simple_client.launch test_client.cpp)
8+
add_dependencies(dynamic_reconfigure-test_client ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_py)
9+
target_link_libraries(dynamic_reconfigure-test_client pthread dynamic_reconfigure_config_init_mutex ${catkin_LIBRARIES})
10+
11+
add_dependencies(tests dynamic_reconfigure-test_client)
12+
713
add_rostest(test_python_simple_client.launch)

0 commit comments

Comments
 (0)