combined_robot_hw.cpp
Go to the documentation of this file.
1 // Copyright (C) 2015, Shadow Robot Company Ltd.
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 name of Shadow Robot Company Ltd. 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 
28 #include <algorithm>
30 
31 namespace combined_robot_hw
32 {
34  robot_hw_loader_("hardware_interface", "hardware_interface::RobotHW")
35  {}
36 
38  {
39  root_nh_ = root_nh;
40  robot_hw_nh_ = robot_hw_nh;
41 
42  std::vector<std::string> robots;
43  std::string param_name = "robot_hardware";
44  if (!robot_hw_nh.getParam(param_name, robots))
45  {
46  ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << robot_hw_nh.getNamespace() << ").");
47  return false;
48  }
49 
50  std::vector<std::string>::iterator it;
51  for (it = robots.begin(); it != robots.end(); it++)
52  {
53  if (!loadRobotHW(*it))
54  {
55  return false;
56  }
57  }
58  return true;
59  }
60 
61  bool CombinedRobotHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
62  const std::list<hardware_interface::ControllerInfo>& stop_list)
63  {
64  // Call the prepareSwitch method of the single RobotHW objects.
65  std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
66  for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
67  {
68  std::list<hardware_interface::ControllerInfo> filtered_start_list;
69  std::list<hardware_interface::ControllerInfo> filtered_stop_list;
70 
71  // Generate a filtered version of start_list and stop_list for each RobotHW before calling prepareSwitch
72  filterControllerList(start_list, filtered_start_list, *robot_hw);
73  filterControllerList(stop_list, filtered_stop_list, *robot_hw);
74 
75  if (!(*robot_hw)->prepareSwitch(filtered_start_list, filtered_stop_list))
76  return false;
77  }
78  return true;
79  }
80 
81  void CombinedRobotHW::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
82  const std::list<hardware_interface::ControllerInfo>& stop_list)
83  {
84  // Call the doSwitch method of the single RobotHW objects.
85  std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
86  for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
87  {
88  std::list<hardware_interface::ControllerInfo> filtered_start_list;
89  std::list<hardware_interface::ControllerInfo> filtered_stop_list;
90 
91  // Generate a filtered version of start_list and stop_list for each RobotHW before calling doSwitch
92  filterControllerList(start_list, filtered_start_list, *robot_hw);
93  filterControllerList(stop_list, filtered_stop_list, *robot_hw);
94 
95  (*robot_hw)->doSwitch(filtered_start_list, filtered_stop_list);
96  }
97  }
98 
99  bool CombinedRobotHW::loadRobotHW(const std::string& name)
100  {
101  ROS_DEBUG("Will load robot HW '%s'", name.c_str());
102 
103  ros::NodeHandle c_nh;
104  // Constructs the robot HW
105  try
106  {
107  c_nh = ros::NodeHandle(robot_hw_nh_, name);
108  }
109  catch(std::exception &e)
110  {
111  ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s':\n%s", name.c_str(), e.what());
112  return false;
113  }
114  catch(...)
115  {
116  ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s'", name.c_str());
117  return false;
118  }
119 
121  std::string type;
122  if (c_nh.getParam("type", type))
123  {
124  ROS_DEBUG("Constructing robot HW '%s' of type '%s'", name.c_str(), type.c_str());
125  try
126  {
127  std::vector<std::string> cur_types = robot_hw_loader_.getDeclaredClasses();
128  for(size_t i=0; i < cur_types.size(); i++)
129  {
130  if (type == cur_types[i])
131  {
132  robot_hw = robot_hw_loader_.createInstance(type);
133  }
134  }
135  }
136  catch (const std::runtime_error &ex)
137  {
138  ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
139  }
140  }
141  else
142  {
143  ROS_ERROR("Could not load robot HW '%s' because the type was not specified. Did you load the robot HW configuration on the parameter server (namespace: '%s')?", name.c_str(), c_nh.getNamespace().c_str());
144  return false;
145  }
146 
147  // checks if robot HW was constructed
148  if (!robot_hw)
149  {
150  ROS_ERROR("Could not load robot HW '%s' because robot HW type '%s' does not exist.", name.c_str(), type.c_str());
151  return false;
152  }
153 
154  // Initializes the robot HW
155  ROS_DEBUG("Initializing robot HW '%s'", name.c_str());
156  bool initialized;
157  try
158  {
159  initialized = robot_hw->init(root_nh_, c_nh);
160  }
161  catch(std::exception &e)
162  {
163  ROS_ERROR("Exception thrown while initializing robot HW %s.\n%s", name.c_str(), e.what());
164  initialized = false;
165  }
166  catch(...)
167  {
168  ROS_ERROR("Exception thrown while initializing robot HW %s", name.c_str());
169  initialized = false;
170  }
171 
172  if (!initialized)
173  {
174  ROS_ERROR("Initializing robot HW '%s' failed", name.c_str());
175  return false;
176  }
177  ROS_DEBUG("Initialized robot HW '%s' successful", name.c_str());
178 
179  robot_hw_list_.push_back(robot_hw);
180 
181  this->registerInterfaceManager(robot_hw.get());
182 
183  ROS_DEBUG("Successfully load robot HW '%s'", name.c_str());
184  return true;
185  }
186 
187  void CombinedRobotHW::read(const ros::Time& time, const ros::Duration& period)
188  {
189  // Call the read method of the single RobotHW objects.
190  std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
191  for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
192  {
193  (*robot_hw)->read(time, period);
194  }
195  }
196 
197 
198  void CombinedRobotHW::write(const ros::Time& time, const ros::Duration& period)
199  {
200  // Call the write method of the single RobotHW objects.
201  std::vector<hardware_interface::RobotHWSharedPtr>::iterator robot_hw;
202  for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw)
203  {
204  (*robot_hw)->write(time, period);
205  }
206  }
207 
208  void CombinedRobotHW::filterControllerList(const std::list<hardware_interface::ControllerInfo>& list,
209  std::list<hardware_interface::ControllerInfo>& filtered_list,
211  {
212  filtered_list.clear();
213  for (std::list<hardware_interface::ControllerInfo>::const_iterator it = list.begin(); it != list.end(); ++it)
214  {
215  hardware_interface::ControllerInfo filtered_controller;
216  filtered_controller.name = it->name;
217  filtered_controller.type = it->type;
218 
219  if (it->claimed_resources.empty())
220  {
221  filtered_list.push_back(filtered_controller);
222  continue;
223  }
224  for (std::vector<hardware_interface::InterfaceResources>::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it)
225  {
226  hardware_interface::InterfaceResources filtered_iface_resources;
227  filtered_iface_resources.hardware_interface = res_it->hardware_interface;
228  std::vector<std::string> r_hw_ifaces = robot_hw->getNames();
229 
230  std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), filtered_iface_resources.hardware_interface);
231  if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered in r_hw, so we filter it out
232  {
233  continue;
234  }
235 
236  std::vector<std::string> r_hw_iface_resources = robot_hw->getInterfaceResources(filtered_iface_resources.hardware_interface);
237  std::set<std::string> filtered_resources;
238  for (std::set<std::string>::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res)
239  {
240  std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res);
241  if (res_name != r_hw_iface_resources.end())
242  {
243  filtered_resources.insert(*ctrl_res);
244  }
245  }
246  if (!filtered_resources.empty())
247  {
248  filtered_iface_resources.resources = filtered_resources;
249  filtered_controller.claimed_resources.push_back(filtered_iface_resources);
250  }
251  }
252  if (!filtered_controller.claimed_resources.empty())
253  {
254  filtered_list.push_back(filtered_controller);
255  }
256  }
257  }
258 }
std::vector< hardware_interface::RobotHWSharedPtr > robot_hw_list_
pluginlib::ClassLoader< hardware_interface::RobotHW > robot_hw_loader_
void registerInterfaceManager(InterfaceManager *iface_man)
virtual bool prepareSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
std::set< std::string > resources
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 loadRobotHW(const std::string &name)
std::vector< InterfaceResources > claimed_resources
std::vector< std::string > getDeclaredClasses()
virtual void read(const ros::Time &time, const ros::Duration &period)
const std::string & getNamespace() const
virtual void doSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
bool getParam(const std::string &key, std::string &s) const
virtual void write(const ros::Time &time, const ros::Duration &period)
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
void filterControllerList(const std::list< hardware_interface::ControllerInfo > &list, std::list< hardware_interface::ControllerInfo > &filtered_list, hardware_interface::RobotHWSharedPtr robot_hw)
Filters the start and stop lists so that they only contain the controllers and resources that corresp...
#define ROS_DEBUG(...)


combined_robot_hw
Author(s): Toni Oliver
autogenerated on Mon Apr 20 2020 03:52:07