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 }
void set_last_uint64_value(const uint64_t new_last_uint64_value)
Set last_uint64_value value.
SetUint64Message Fawkes BlackBoard Interface Message.
SetStringMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
SetFloatMessage Fawkes BlackBoard Interface Message.
virtual void finalize()
Method for finalization.
void set_last_msg_id(const uint64_t new_last_msg_id)
Set last_msg_id value.
Fawkes library namespace.
SetBoolMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
void set_last_bool_value(const bool new_last_bool_value)
Set last_bool_value value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
Base class for exceptions in Fawkes.
Definition: exception.h:35
SetUint32Message Fawkes BlackBoard Interface Message.
void set_last_msg_status(const LastMsgStatus new_last_msg_status)
Set last_msg_status value.
void set_last_float_value(const float new_last_float_value)
Set last_float_value value.
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void set_last_str_value(const char *new_last_str_value)
Set last_str_value value.
virtual void init()
Method for initialization.
void set_last_uint32_value(const uint32_t new_last_uint32_value)
Set last_uint32_value value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
void set_last_parameter(const char *new_last_parameter)
Set last_parameter value.
DynamicReconfigureInterface Fawkes BlackBoard Interface.
void set_last_service(const char *new_last_service)
Set last_service value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.