multi_interface_controller.h
Go to the documentation of this file.
1 // Copyright (C) 2015, 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 names of PAL Robotics S.L. 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 
30 #ifndef CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H
31 #define CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H
32 
33 #include <algorithm>
34 #include <sstream>
35 
40 #include <ros/ros.h>
41 
42 namespace controller_interface
43 {
44 
46 namespace internal
47 {
48 
49 template <class T>
51 
52 template <class T>
54 
55 template <class T>
57  hardware_interface::RobotHW* robot_hw_out);
58 
59 template <class T>
61  ControllerBase::ClaimedResources& claimed_resources);
62 
63 template <class T>
64 std::string enumerateElements(const T& val,
65  const std::string& delimiter = ", ",
66  const std::string& prefix = "",
67  const std::string& suffix = "");
68 
69 } // namespace
193 template <class T1, class T2 = void, class T3 = void, class T4 = void>
195 {
196 public:
202  MultiInterfaceController(bool allow_optional_interfaces = false)
203  : allow_optional_interfaces_(allow_optional_interfaces)
204  {state_ = CONSTRUCTED;}
205 
207 
235  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
236  ros::NodeHandle& /*controller_nh*/)
237  {return true;}
238 
265  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
266  ros::NodeHandle& /*root_nh*/,
267  ros::NodeHandle& /*controller_nh*/)
268  {return true;}
269 
270 protected:
293  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
294  ros::NodeHandle& root_nh,
295  ros::NodeHandle& controller_nh,
296  ClaimedResources& claimed_resources)
297  {
298  // check if construction finished cleanly
299  if (state_ != CONSTRUCTED){
300  ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
301  return false;
302  }
303 
304  // check for required hardware interfaces
305  if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;}
306 
307  // populate robot hardware abstraction containing only controller hardware interfaces (subset of robot)
308  hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_;
309  extractInterfaceResources(robot_hw, robot_hw_ctrl_p);
310 
311  // custom controller initialization
312  clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init
313  if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh))
314  {
315  ROS_ERROR("Failed to initialize the controller");
316  return false;
317  }
318 
319  // populate claimed resources
320  claimed_resources.clear();
321  populateClaimedResources(robot_hw_ctrl_p, claimed_resources);
322  clearClaims(robot_hw_ctrl_p);
323  // NOTE: Above, claims are cleared since we only want to know what they are and report them back
324  // as an output parameter. Actual resource claiming by the controller is done when the controller
325  // is start()ed
326 
327  // initialization successful
328  state_ = INITIALIZED;
329  return true;
330  }
331 
332  /*\}*/
333 
341  {
343  return hasInterface<T1>(robot_hw) &&
344  hasInterface<T2>(robot_hw) &&
345  hasInterface<T3>(robot_hw) &&
346  hasInterface<T4>(robot_hw);
347  }
348 
355  {
356  using internal::clearClaims;
357  clearClaims<T1>(robot_hw);
358  clearClaims<T2>(robot_hw);
359  clearClaims<T3>(robot_hw);
360  clearClaims<T4>(robot_hw);
361  }
362 
372  hardware_interface::RobotHW* robot_hw_out)
373  {
375  extractInterfaceResources<T1>(robot_hw_in, robot_hw_out);
376  extractInterfaceResources<T2>(robot_hw_in, robot_hw_out);
377  extractInterfaceResources<T3>(robot_hw_in, robot_hw_out);
378  extractInterfaceResources<T4>(robot_hw_in, robot_hw_out);
379  }
380 
390  ClaimedResources& claimed_resources)
391  {
393  populateClaimedResources<T1>(robot_hw, claimed_resources);
394  populateClaimedResources<T2>(robot_hw, claimed_resources);
395  populateClaimedResources<T3>(robot_hw, claimed_resources);
396  populateClaimedResources<T4>(robot_hw, claimed_resources);
397  }
398 
401 
404 
405 private:
408 };
409 
410 
411 namespace internal
412 {
413 
414 template <class T>
416 {
417  T* hw = robot_hw->get<T>();
418  if (!hw)
419  {
420  const std::string hw_name = hardware_interface::internal::demangledTypeName<T>();
421  ROS_ERROR_STREAM("This controller requires a hardware interface of type '" << hw_name << "', " <<
422  "but is not exposed by the robot. Available interfaces in robot:\n" <<
423  enumerateElements(robot_hw->getNames(), "\n", "- '", "'")); // delimiter, prefix, suffux
424  return false;
425  }
426  return true;
427 }
428 
429 // Specialization for unused template parameters defaulting to void
430 template <>
431 inline bool hasInterface<void>(hardware_interface::RobotHW* /*robot_hw*/) {return true;}
432 
433 template <class T>
435 {
436  T* hw = robot_hw->get<T>();
437  if (hw) {hw->clearClaims();}
438 }
439 
440 // Specialization for unused template parameters defaulting to void
441 template <>
442 inline void clearClaims<void>(hardware_interface::RobotHW* /*robot_hw*/) {}
443 
444 template <class T>
446  hardware_interface::RobotHW* robot_hw_out)
447 {
448  T* hw = robot_hw_in->get<T>();
449  if (hw) {robot_hw_out->registerInterface(hw);}
450 }
451 
452 // Specialization for unused template parameters defaulting to void
453 template <>
455  hardware_interface::RobotHW* /*robot_hw_out*/) {}
456 
457 template <class T>
458 inline void populateClaimedResources(hardware_interface::RobotHW* robot_hw,
459  ControllerBase::ClaimedResources& claimed_resources)
460 {
461  T* hw = robot_hw->get<T>();
462  if (hw)
463  {
465  iface_res.hardware_interface = hardware_interface::internal::demangledTypeName<T>();
466  iface_res.resources = hw->getClaims();
467  claimed_resources.push_back(iface_res);
468  }
469 }
470 
471 // Specialization for unused template parameters defaulting to void
472 template <>
473 inline void populateClaimedResources<void>(hardware_interface::RobotHW* /*robot_hw*/,
474  ControllerBase::ClaimedResources& /*claimed_resources*/) {}
475 
476 template <class T>
477 inline std::string enumerateElements(const T& val,
478  const std::string& delimiter,
479  const std::string& prefix,
480  const std::string& suffix)
481 {
482  std::string ret;
483  if (val.empty()) {return ret;}
484 
485  const std::string sdp = suffix+delimiter+prefix;
486  std::stringstream ss;
487  ss << prefix;
488  std::copy(val.begin(), val.end(), std::ostream_iterator<typename T::value_type>(ss, sdp.c_str()));
489  ret = ss.str();
490  if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());}
491  return ret;
492 }
493 
494 } // namespace
495 
496 } // namespace
497 
498 #endif
virtual bool init(hardware_interface::RobotHW *, ros::NodeHandle &, ros::NodeHandle &)
Custom controller initialization logic.
void populateClaimedResources(hardware_interface::RobotHW *robot_hw, ControllerBase::ClaimedResources &claimed_resources)
static bool hasRequiredInterfaces(hardware_interface::RobotHW *robot_hw)
Check if robot hardware abstraction contains all required interfaces.
static void extractInterfaceResources(hardware_interface::RobotHW *robot_hw_in, hardware_interface::RobotHW *robot_hw_out)
Extract all hardware interfaces requested by this controller from robot_hw_in, and add them also to r...
std::set< std::string > resources
void init(const M_string &remappings)
bool hasInterface< void >(hardware_interface::RobotHW *)
std::vector< std::string > getNames() const
std::vector< hardware_interface::InterfaceResources > ClaimedResources
void clearClaims(hardware_interface::RobotHW *robot_hw)
void clearClaims< void >(hardware_interface::RobotHW *)
Abstract Controller Interface.
virtual bool initRequest(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources)
Initialize the controller from a RobotHW pointer.
static void clearClaims(hardware_interface::RobotHW *robot_hw)
Clear claims from all hardware interfaces requested by this controller.
static void populateClaimedResources(hardware_interface::RobotHW *robot_hw, ClaimedResources &claimed_resources)
Extract all hardware interfaces requested by this controller from robot_hw_in, and add them also to r...
void extractInterfaceResources< void >(hardware_interface::RobotHW *, hardware_interface::RobotHW *)
bool hasInterface(hardware_interface::RobotHW *robot_hw)
MultiInterfaceController(bool allow_optional_interfaces=false)
void extractInterfaceResources(hardware_interface::RobotHW *robot_hw_in, hardware_interface::RobotHW *robot_hw_out)
void populateClaimedResources< void >(hardware_interface::RobotHW *, ControllerBase::ClaimedResources &)
Controller able to claim resources from multiple hardware interfaces.
virtual bool init(hardware_interface::RobotHW *, ros::NodeHandle &)
Custom controller initialization logic.
std::string enumerateElements(const T &val, const std::string &delimiter, const std::string &prefix, const std::string &suffix)
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)


controller_interface
Author(s): Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:06