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 #pragma once
31 
32 
38 #include <ros/ros.h>
39 
40 namespace controller_interface
41 {
42 
148 template <typename... T>
150 {
151 public:
157  MultiInterfaceController(bool allow_optional_interfaces = false)
158  : allow_optional_interfaces_(allow_optional_interfaces) {}
159 
187  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
188  ros::NodeHandle& /*controller_nh*/)
189  {return true;}
190 
217  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
218  ros::NodeHandle& /*root_nh*/,
219  ros::NodeHandle& /*controller_nh*/)
220  {return true;}
221 
222 protected:
246  ros::NodeHandle& root_nh,
247  ros::NodeHandle& controller_nh,
248  ClaimedResources& claimed_resources) override
249  {
250  // check if construction finished cleanly
251  if (state_ != CONSTRUCTED){
252  ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
253  return false;
254  }
255 
256  // check for required hardware interfaces
257  if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;}
258 
259  // populate robot hardware abstraction containing only controller hardware interfaces (subset of robot)
260  hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_;
261  extractInterfaceResources(robot_hw, robot_hw_ctrl_p);
262 
263  // custom controller initialization
264  clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init
265  if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh))
266  {
267  ROS_ERROR("Failed to initialize the controller");
268  return false;
269  }
270 
271  // populate claimed resources
272  claimed_resources.clear();
273  populateClaimedResources(robot_hw_ctrl_p, claimed_resources);
274  clearClaims(robot_hw_ctrl_p);
275  // NOTE: Above, claims are cleared since we only want to know what they are and report them back
276  // as an output parameter. Actual resource claiming by the controller is done when the controller
277  // is start()ed
278 
279  // initialization successful
281  return true;
282  }
283 
284  /*\}*/
285 
293  {
294  return internal::hasInterfaces<T...>(robot_hw);
295  }
296 
303  {
304  internal::clearClaims<T...>(robot_hw);
305  }
306 
316  hardware_interface::RobotHW* robot_hw_out)
317  {
318  internal::extractInterfaceResources<T...>(robot_hw_in, robot_hw_out);
319  }
320 
330  ClaimedResources& claimed_resources)
331  {
332  internal::populateClaimedResources<T...>(robot_hw, claimed_resources);
333  }
334 
337 
340 };
341 
342 } // namespace
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...
std::vector< hardware_interface::InterfaceResources > ClaimedResources
virtual bool init(hardware_interface::RobotHW *, ros::NodeHandle &, ros::NodeHandle &)
Custom controller initialization logic.
virtual bool init(hardware_interface::RobotHW *, ros::NodeHandle &)
Custom controller initialization logic.
Abstract Controller Interface.
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...
static bool hasRequiredInterfaces(hardware_interface::RobotHW *robot_hw)
Check if robot hardware abstraction contains all required interfaces.
enum controller_interface::ControllerBase::@0 state_
The current execution state of the controller.
bool initRequest(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) override
Initialize the controller from a RobotHW pointer.
Controller able to claim resources from multiple hardware interfaces.
MultiInterfaceController(bool allow_optional_interfaces=false)
#define ROS_ERROR(...)
static void clearClaims(hardware_interface::RobotHW *robot_hw)
Clear claims from all hardware interfaces requested by this controller.


controller_interface
Author(s): Wim Meeussen
autogenerated on Thu Oct 1 2020 03:19:36