30 #ifndef CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H 31 #define CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H 65 const std::string& delimiter =
", ",
66 const std::string& prefix =
"",
67 const std::string& suffix =
"");
193 template <
class T1,
class T2 =
void,
class T3 =
void,
class T4 =
void>
203 : allow_optional_interfaces_(allow_optional_interfaces)
204 {state_ = CONSTRUCTED;}
299 if (state_ != CONSTRUCTED){
300 ROS_ERROR(
"Cannot initialize this controller because it failed to be constructed");
305 if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {
return false;}
313 if (!
init(robot_hw_ctrl_p, controller_nh) || !
init(robot_hw_ctrl_p, root_nh, controller_nh))
315 ROS_ERROR(
"Failed to initialize the controller");
320 claimed_resources.clear();
328 state_ = INITIALIZED;
343 return hasInterface<T1>(robot_hw) &&
344 hasInterface<T2>(robot_hw) &&
345 hasInterface<T3>(robot_hw) &&
346 hasInterface<T4>(robot_hw);
357 clearClaims<T1>(robot_hw);
358 clearClaims<T2>(robot_hw);
359 clearClaims<T3>(robot_hw);
360 clearClaims<T4>(robot_hw);
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);
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);
417 T* hw = robot_hw->
get<T>();
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" <<
436 T* hw = robot_hw->
get<T>();
437 if (hw) {hw->clearClaims();}
448 T* hw = robot_hw_in->
get<T>();
455 hardware_interface::RobotHW* ) {}
461 T* hw = robot_hw->
get<T>();
467 claimed_resources.push_back(iface_res);
478 const std::string& delimiter,
479 const std::string& prefix,
480 const std::string& suffix)
483 if (val.empty()) {
return ret;}
485 const std::string sdp = suffix+delimiter+prefix;
486 std::stringstream ss;
488 std::copy(val.begin(), val.end(), std::ostream_iterator<typename T::value_type>(ss, sdp.c_str()));
490 if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());}
void registerInterface(T *iface)
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)
virtual ~MultiInterfaceController()
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)
bool allow_optional_interfaces_
hardware_interface::RobotHW robot_hw_ctrl_
std::string hardware_interface