Fawkes API  Fawkes Development Version
dynamic_reconfigure_thread.cpp
1 
2 /***************************************************************************
3  * dynamic_reconfigure_thread.cpp - Robotino ROS Dynamic Reconfigure Thread
4  *
5  * Created: Fri May 05 20:07:27 2017
6  * Copyright 2017 Christoph Henke
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "dynamic_reconfigure_thread.h"
23 
24 #include <dynamic_reconfigure/BoolParameter.h>
25 #include <dynamic_reconfigure/Config.h>
26 #include <dynamic_reconfigure/DoubleParameter.h>
27 #include <dynamic_reconfigure/IntParameter.h>
28 #include <dynamic_reconfigure/Reconfigure.h>
29 #include <dynamic_reconfigure/StrParameter.h>
30 
31 using namespace fawkes;
32 
33 /** @class RosDynamicReconfigureThread "dynamic_reconfigure_thread.h"
34  * Performing dynamic reconfiguration between Fawkes and ROS
35  * @author Christoph Henke
36  */
37 
38 /** Contructor. */
40 : Thread("RosDynamicReconfigureThread", Thread::OPMODE_WAITFORWAKEUP),
42 {
43 }
44 
45 /** Method for initialization
46  */
47 void
49 {
50  try {
51  dynrec_if_ = blackboard->open_for_writing<DynamicReconfigureInterface>("DynamicReconfigure");
52  } catch (Exception &e) {
53  e.append("%s initialization failed, could not open dynamic reconfigure "
54  "interface for writing",
55  name());
56  logger->log_error(name(), e);
57  throw;
58  }
59 
60  // Initialize interface
61  dynrec_if_->set_last_service("");
62  dynrec_if_->set_last_parameter("");
63  dynrec_if_->set_last_msg_id(0);
64  dynrec_if_->set_last_bool_value(false);
65  dynrec_if_->set_last_str_value("");
66  dynrec_if_->set_last_uint32_value(0);
67  dynrec_if_->set_last_uint64_value(0);
68  dynrec_if_->set_last_float_value(0.0);
69  dynrec_if_->write();
70 }
71 
72 /** Method for finalization
73  */
74 void
76 {
77  // close interfaces
78  try {
79  blackboard->close(dynrec_if_);
80  } catch (Exception &e) {
81  logger->log_error(name(), "Closing interface failed!");
82  logger->log_error(name(), e);
83  }
84 }
85 
86 /** Method for setting an dynamic reconfigure of type bool
87  * @param service the service to call for dynamic reconfiguration
88  * @param path path of the ROS parameter
89  * @param value value for the ROS parameter
90  */
91 bool
92 RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
93  const std::string &parameter,
94  const bool value)
95 {
96  dynamic_reconfigure::BoolParameter dynreconf_bool_param;
97  dynamic_reconfigure::Config dynreconf_conf;
98  dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
99  dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
100 
101  dynreconf_bool_param.name = parameter;
102  dynreconf_bool_param.value = value;
103  dynreconf_conf.bools.push_back(dynreconf_bool_param);
104  dynreconf_srv_req.config = dynreconf_conf;
105 
106  if (!ros::service::exists(service, false)) {
107  logger->log_error(name(),
108  "Error in setting dynreconf value %s to %s - "
109  "service %s does not exists",
110  parameter.c_str(),
111  value ? "true" : "false",
112  service.c_str());
113  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
114  dynrec_if_->write();
115  dynreconf_conf.bools.clear();
116  return false;
117  } else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
118  logger->log_error(name(),
119  "Error in setting dynreconf value %s to %s with service %s",
120  parameter.c_str(),
121  value ? "true" : "false",
122  service.c_str());
123  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
124  dynrec_if_->write();
125  dynreconf_conf.bools.clear();
126  return false;
127  } else {
128  logger->log_info(name(), "Setting %s to %s", parameter.c_str(), value ? "true" : "false");
129  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
130  dynrec_if_->write();
131  dynreconf_conf.bools.clear();
132  return true;
133  }
134 }
135 
136 /** Method for setting an dynamic reconfigure of type string
137  * @param service the service to call for dynamic reconfiguration
138  * @param path path of the ROS parameter
139  * @param value value for the ROS parameter
140  */
141 bool
142 RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
143  const std::string &parameter,
144  const std::string &value)
145 {
146  dynamic_reconfigure::StrParameter dynreconf_str_param;
147  dynamic_reconfigure::Config dynreconf_conf;
148  dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
149  dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
150 
151  dynreconf_str_param.name = parameter;
152  dynreconf_str_param.value = value;
153  dynreconf_conf.strs.push_back(dynreconf_str_param);
154  dynreconf_srv_req.config = dynreconf_conf;
155 
156  if (!ros::service::exists(service, false)) {
157  logger->log_error(name(),
158  "Error in setting dynreconf value %s to %s - "
159  "service %s does not exists",
160  parameter.c_str(),
161  value.c_str(),
162  service.c_str());
163  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
164  dynrec_if_->write();
165  dynreconf_conf.strs.clear();
166  return false;
167  } else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
168  logger->log_error(name(),
169  "Error in setting dynreconf value %s to %s with service %s",
170  parameter.c_str(),
171  value.c_str(),
172  service.c_str());
173  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
174  dynrec_if_->write();
175  dynreconf_conf.strs.clear();
176  return false;
177  } else {
178  logger->log_info(name(), "Setting %s to %s", parameter.c_str(), value.c_str());
179  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
180  dynrec_if_->write();
181  dynreconf_conf.strs.clear();
182  return true;
183  }
184 }
185 
186 /** Method for setting an dynamic reconfigure of type int
187  * @param service the service to call for dynamic reconfiguration
188  * @param path path of the ROS parameter
189  * @param value value for the ROS parameter
190  */
191 bool
192 RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
193  const std::string &parameter,
194  const int value)
195 {
196  dynamic_reconfigure::IntParameter dynreconf_int_param;
197  dynamic_reconfigure::Config dynreconf_conf;
198  dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
199  dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
200 
201  dynreconf_int_param.name = parameter;
202  dynreconf_int_param.value = value;
203  dynreconf_conf.ints.push_back(dynreconf_int_param);
204  dynreconf_srv_req.config = dynreconf_conf;
205 
206  if (!ros::service::exists(service, false)) {
207  logger->log_error(name(),
208  "Error in setting dynreconf value %s to %d - "
209  "service %s does not exists",
210  parameter.c_str(),
211  value,
212  service.c_str());
213  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
214  dynrec_if_->write();
215  dynreconf_conf.ints.clear();
216  return false;
217  } else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
218  logger->log_error(name(),
219  "Error in setting dynreconf value %s to %d with service %s",
220  parameter.c_str(),
221  value,
222  service.c_str());
223  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
224  dynrec_if_->write();
225  dynreconf_conf.ints.clear();
226  return false;
227  } else {
228  logger->log_info(name(), "Setting %s to %d", parameter.c_str(), value);
229  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
230  dynrec_if_->write();
231  dynreconf_conf.ints.clear();
232  return true;
233  }
234 }
235 
236 /** Method for setting an dynamic reconfigure of type float
237  * @param service the service to call for dynamic reconfiguration
238  * @param path path of the ROS parameter
239  * @param value value for the ROS parameter
240  */
241 bool
242 RosDynamicReconfigureThread::set_dynreconf_value(const std::string &service,
243  const std::string &parameter,
244  const double value)
245 {
246  dynamic_reconfigure::DoubleParameter dynreconf_double_param;
247  dynamic_reconfigure::Config dynreconf_conf;
248  dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
249  dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
250 
251  dynreconf_double_param.name = parameter;
252  dynreconf_double_param.value = value;
253  dynreconf_conf.doubles.push_back(dynreconf_double_param);
254  dynreconf_srv_req.config = dynreconf_conf;
255 
256  if (!ros::service::exists(service, false)) {
257  logger->log_error(name(),
258  "Error in setting dynreconf value %s to %f - "
259  "service %s does not exists",
260  parameter.c_str(),
261  value,
262  service.c_str());
263  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
264  dynrec_if_->write();
265  dynreconf_conf.doubles.clear();
266  return false;
267  } else if (!ros::service::call(service, dynreconf_srv_req, dynreconf_srv_resp)) {
268  logger->log_error(name(),
269  "Error in setting dynreconf value %s to %f with service %s",
270  parameter.c_str(),
271  value,
272  service.c_str());
273  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Failed);
274  dynrec_if_->write();
275  dynreconf_conf.doubles.clear();
276  return false;
277  } else {
278  logger->log_info(name(), "Setting %s to %f", parameter.c_str(), value);
279  dynrec_if_->set_last_msg_status(fawkes::DynamicReconfigureInterface::LastMsgStatus::Succeeded);
280  dynrec_if_->write();
281  dynreconf_conf.doubles.clear();
282  return true;
283  }
284 }
285 
286 /** Method for resetting all interface values
287  */
288 void
289 RosDynamicReconfigureThread::reset_dynamic_reconfigure_interface()
290 {
291  dynrec_if_->set_last_service("");
292  dynrec_if_->set_last_parameter("");
293  dynrec_if_->set_last_msg_id(0);
294  dynrec_if_->set_last_bool_value(false);
295  dynrec_if_->set_last_str_value("");
296  dynrec_if_->set_last_uint32_value(0);
297  dynrec_if_->set_last_uint64_value(0);
298  dynrec_if_->set_last_float_value(0.0);
299 }
300 
301 void
303 {
304  while (!dynrec_if_->msgq_empty()) {
305  if (DynamicReconfigureInterface::SetBoolMessage *msg = dynrec_if_->msgq_first_safe(msg)) {
306  logger->log_info(name(), "Bool message received");
307  reset_dynamic_reconfigure_interface();
308  // Writing the last called service and the according parameters into the blackboard
309  dynrec_if_->set_last_service(msg->service());
310  dynrec_if_->set_last_parameter(msg->parameter());
311  dynrec_if_->set_last_msg_id(msg->id());
312  dynrec_if_->set_last_bool_value(msg->is_value());
313  dynrec_if_->write();
314  set_dynreconf_value(msg->service(), msg->parameter(), msg->is_value());
316  dynrec_if_->msgq_first_safe(msg)) {
317  logger->log_info(name(), "String message received");
318  reset_dynamic_reconfigure_interface();
319  // Writing the last called service and the according parameters into the blackboard
320  dynrec_if_->set_last_service(msg->service());
321  dynrec_if_->set_last_parameter(msg->parameter());
322  dynrec_if_->set_last_msg_id(msg->id());
323  dynrec_if_->set_last_str_value(msg->value());
324  dynrec_if_->write();
325  set_dynreconf_value(msg->service(), msg->parameter(), msg->value());
327  dynrec_if_->msgq_first_safe(msg)) {
328  logger->log_info(name(), "Uint32 message received");
329  reset_dynamic_reconfigure_interface();
330  // Writing the last called service and the according parameters into the blackboard
331  dynrec_if_->set_last_service(msg->service());
332  dynrec_if_->set_last_parameter(msg->parameter());
333  dynrec_if_->set_last_msg_id(msg->id());
334  dynrec_if_->set_last_uint32_value(msg->value());
335  dynrec_if_->write();
336  set_dynreconf_value(msg->service(), msg->parameter(), (int)msg->value());
338  dynrec_if_->msgq_first_safe(msg)) {
339  logger->log_info(name(), "Uint64 message received");
340  reset_dynamic_reconfigure_interface();
341  // Writing the last called service and the according parameters into the blackboard
342  dynrec_if_->set_last_service(msg->service());
343  dynrec_if_->set_last_parameter(msg->parameter());
344  dynrec_if_->set_last_msg_id(msg->id());
345  dynrec_if_->set_last_uint64_value(msg->value());
346  dynrec_if_->write();
347  set_dynreconf_value(msg->service(), msg->parameter(), (int)msg->value());
349  dynrec_if_->msgq_first_safe(msg)) {
350  logger->log_info(name(), "Float message received");
351  reset_dynamic_reconfigure_interface();
352  // Writing the last called service and the according parameters into the blackboard
353  dynrec_if_->set_last_service(msg->service());
354  dynrec_if_->set_last_parameter(msg->parameter());
355  dynrec_if_->set_last_msg_id(msg->id());
356  dynrec_if_->set_last_float_value(msg->value());
357  dynrec_if_->write();
358  set_dynreconf_value(msg->service(), msg->parameter(), (double)msg->value());
359  }
360 
361  dynrec_if_->msgq_pop();
362  } // while
363 }
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Method for finalization.
virtual void init()
Method for initialization.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
SetBoolMessage Fawkes BlackBoard Interface Message.
SetFloatMessage Fawkes BlackBoard Interface Message.
SetStringMessage Fawkes BlackBoard Interface Message.
SetUint32Message Fawkes BlackBoard Interface Message.
SetUint64Message Fawkes BlackBoard Interface Message.
DynamicReconfigureInterface Fawkes BlackBoard Interface.
void set_last_uint64_value(const uint64_t new_last_uint64_value)
Set last_uint64_value value.
void set_last_bool_value(const bool new_last_bool_value)
Set last_bool_value value.
void set_last_parameter(const char *new_last_parameter)
Set last_parameter value.
void set_last_msg_status(const LastMsgStatus new_last_msg_status)
Set last_msg_status value.
void set_last_str_value(const char *new_last_str_value)
Set last_str_value value.
void set_last_float_value(const float new_last_float_value)
Set last_float_value value.
void set_last_service(const char *new_last_service)
Set last_service value.
void set_last_msg_id(const uint64_t new_last_msg_id)
Set last_msg_id value.
void set_last_uint32_value(const uint32_t new_last_uint32_value)
Set last_uint32_value value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.