28 #ifndef HARDWARE_INTERFACE_ROBOT_HW_H 29 #define HARDWARE_INTERFACE_ROBOT_HW_H 94 typedef std::map<std::string, std::list<ControllerInfo> >
ResourceMap;
96 typedef std::list<ControllerInfo>::const_iterator CtrlInfoIt;
97 typedef std::vector<InterfaceResources>::const_iterator ClaimedResIt;
98 typedef std::set<std::string>::const_iterator ResourceIt;
102 ResourceMap resource_map;
103 for (CtrlInfoIt info_it = info.begin(); info_it != info.end(); ++info_it)
105 const std::vector<InterfaceResources>& c_res = info_it->claimed_resources;
106 for (ClaimedResIt c_res_it = c_res.begin(); c_res_it != c_res.end(); ++c_res_it)
108 const std::set<std::string>& iface_resources = c_res_it->resources;
109 for (ResourceIt resource_it = iface_resources.begin(); resource_it != iface_resources.end(); ++resource_it)
111 resource_map[*resource_it].push_back(*info_it);
117 bool in_conflict =
false;
118 for (ResourceMap::iterator it = resource_map.begin(); it != resource_map.end(); ++it)
120 if (it->second.size() > 1)
122 std::string controller_list;
123 for (CtrlInfoIt controller_it = it->second.begin(); controller_it != it->second.end(); ++controller_it)
124 controller_list += controller_it->name +
", ";
125 ROS_WARN(
"Resource conflict on [%s]. Controllers = [%s]", it->first.c_str(), controller_list.c_str());
141 const std::list<ControllerInfo>& stop_list) {
return true; }
147 virtual void doSwitch(
const std::list<ControllerInfo>& ,
148 const std::list<ControllerInfo>& ) {}
virtual void doSwitch(const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
virtual bool checkForConflict(const std::list< ControllerInfo > &info) const
virtual bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh)
The init function is called to initialize the RobotHW from a non-realtime thread. ...
virtual bool prepareSwitch(const std::list< ControllerInfo > &start_list, const std::list< ControllerInfo > &stop_list)
virtual void read(const ros::Time &time, const ros::Duration &period)
boost::shared_ptr< RobotHW > RobotHWSharedPtr
std::map< std::string, std::vector< std::string > > ResourceMap
Robot Hardware Interface and Resource Manager.
virtual void write(const ros::Time &time, const ros::Duration &period)