transmission_interface_loader.h
Go to the documentation of this file.
1 // Copyright (C) 2013, 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 name 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 
29 
30 #ifndef TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_LOADER_H
31 #define TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_LOADER_H
32 
33 // C++ standard
34 #include <algorithm>
35 #include <limits>
36 #include <map>
37 #include <string>
38 #include <utility>
39 #include <vector>
40 
41 // Boost
42 #include <boost/foreach.hpp>
43 #include <boost/shared_ptr.hpp>
44 
45 // ROS
46 #include <ros/console.h>
47 
48 // pluginlib
50 
51 // ros_control
58 
65 
66 namespace transmission_interface
67 {
68 
69 namespace internal
70 {
71 // NOTE: Adapted to C++03 from http://www.cplusplus.com/reference/algorithm/is_permutation
72 // TODO: Use Boost's or C++11's implementation, once they become widespread
73 template<class ForwardIt1, class ForwardIt2>
74 bool is_permutation(ForwardIt1 first, ForwardIt1 last,
75  ForwardIt2 d_first)
76 {
77  // skip common prefix
78  std::pair<ForwardIt1, ForwardIt2> pair = std::mismatch(first, last, d_first);
79  first = pair.first; d_first = pair.second;
80 
81  // iterate over the rest, counting how many times each element
82  // from [first, last) appears in [d_first, d_last)
83  if (first != last) {
84  ForwardIt2 d_last = d_first;
85  std::advance(d_last, std::distance(first, last));
86  for (ForwardIt1 i = first; i != last; ++i) {
87  if (i != std::find(first, i, *i)) continue; // already counted this *i
88 
89  int m = std::count(d_first, d_last, *i);
90  if (m==0 || std::count(i, last, *i) != m) {
91  return false;
92  }
93  }
94  }
95  return true;
96 }
97 
98 } // namespace
99 
100 
107 // TODO: Don't assume interfaces?
109 {
111  : position(std::numeric_limits<double>::quiet_NaN()),
112  velocity(std::numeric_limits<double>::quiet_NaN()),
113  effort(std::numeric_limits<double>::quiet_NaN()),
114  position_cmd(std::numeric_limits<double>::quiet_NaN()),
115  velocity_cmd(std::numeric_limits<double>::quiet_NaN()),
116  effort_cmd(std::numeric_limits<double>::quiet_NaN())
117  {}
118 
119  double position;
120  double velocity;
121  double effort;
122  double position_cmd;
123  double velocity_cmd;
124  double effort_cmd;
125 };
126 
127 typedef std::map<std::string, RawJointData> RawJointDataMap;
128 
133 {
138 };
139 
141 {
146 };
147 
149 {
154 };
155 
157 {
158  typedef boost::shared_ptr<Transmission> TransmissionPtr; // DEPRECATED and unused!
159 
161  : robot_hw(0),
162  robot_transmissions(0)
163  {}
164 
168  RawJointDataMap raw_joint_data_map;
171  std::vector<TransmissionSharedPtr> transmission_data;
172 };
173 
174 class RequisiteProvider // TODO: There must be a more descriptive name for this class!
175 {
176 public:
177 
178  virtual ~RequisiteProvider() {}
179 
194  virtual bool updateJointInterfaces(const TransmissionInfo& transmission_info,
195  hardware_interface::RobotHW* robot_hw,
196  JointInterfaces& joint_interfaces,
197  RawJointDataMap& raw_joint_data_map) = 0;
198 
199  bool loadTransmissionMaps(const TransmissionInfo& transmission_info,
200  TransmissionLoaderData& loader_data,
201  TransmissionSharedPtr transmission);
202 protected:
204  {
205  std::string name;
211  };
212 
213  virtual bool getJointStateData(const TransmissionInfo& transmission_info,
214  const RawJointDataMap& raw_joint_data_map,
215  JointData& jnt_state_data) = 0;
216 
217  virtual bool getJointCommandData(const TransmissionInfo& transmission_info,
218  const RawJointDataMap& raw_joint_data_map,
219  JointData& jnt_cmd_data) = 0;
220 
221  virtual bool getActuatorStateData(const TransmissionInfo& transmission_info,
222  hardware_interface::RobotHW* robot_hw,
223  ActuatorData& act_state_data) = 0;
224 
225  virtual bool getActuatorCommandData(const TransmissionInfo& transmission_info,
226  hardware_interface::RobotHW* robot_hw,
227  ActuatorData& act_cmd_data) = 0;
228 
229  virtual bool registerTransmission(TransmissionLoaderData& loader_data,
230  TransmissionHandleData& handle_data) = 0;
231 
232  template <class Interface>
233  static bool hasResource(const std::string& name, const Interface& iface)
234  {
236 
237  // Do nothing if resource already exists on the interface
238  const std::vector<std::string>& existing_resources = iface.getNames();
239  if (existing_resources.end() != std::find(existing_resources.begin(), existing_resources.end(), name))
240  {
241  ROS_DEBUG_STREAM_NAMED("parser", "Resource '" << name << "' already exists on interface '" <<
242  demangledTypeName<Interface>());
243  return true;
244  }
245  else
246  {
247  ROS_DEBUG_STREAM_NAMED("parser", "Resource '" << name << "' does not exist on interface '" <<
248  demangledTypeName<Interface>());
249  return false;
250  }
251  }
252 
253  template <class HardwareInterface, class Handle>
254  bool getActuatorHandles(const std::vector<ActuatorInfo>& actuators_info,
255  hardware_interface::RobotHW* robot_hw,
256  std::vector<Handle>& handles)
257  {
261 
262  HardwareInterface* hw_iface = robot_hw->get<HardwareInterface>();
263 
264  // Check required hardware interface
265  if (!hw_iface)
266  {
267  ROS_ERROR_STREAM_NAMED("parser", "Robot does not have the required hardware interface '" <<
268  demangledTypeName<HardwareInterface>() << "'.");
269  return false;
270  }
271 
272  // Get handles to all required resource
273  BOOST_FOREACH(const ActuatorInfo& info, actuators_info)
274  {
275  try
276  {
277  handles.push_back(hw_iface->getHandle(info.name_));
278  }
279  catch(const HardwareInterfaceException& ex)
280  {
281  ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << info.name_ <<
282  "' does not expose the required hardware interface '" <<
283  demangledTypeName<HardwareInterface>() << "'.");
284  return false;
285  }
286  }
287  return true;
288  }
289 };
290 
365 {
366 public:
372  RobotTransmissions* robot_transmissions);
373 
382  bool load(const std::string& urdf);
383 
392  bool load(const std::vector<TransmissionInfo>& transmission_info_vec);
393 
402  bool load(const TransmissionInfo& transmission_info);
403 
404  TransmissionLoaderData* getData() {return &loader_data_;}
405 
406 private:
411 
413 
414  TransmissionClassLoaderPtr transmission_class_loader_;
415  RequisiteProviderClassLoaderPtr req_provider_loader_;
416 
417 private:
420 
422 };
423 
424 } // namespace
425 
426 #endif // header guard
bool getActuatorHandles(const std::vector< ActuatorInfo > &actuators_info, hardware_interface::RobotHW *robot_hw, std::vector< Handle > &handles)
hardware_interface::RobotHW * robot_hw
Lifecycle is externally controlled (ie. hardware abstraction)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:61
Contains semantic info about a given actuator loaded from XML (URDF)
Parses <transmission> elements into corresponding structs from XML (URDF).
Contains semantic info about a given transmission loaded from XML (URDF)
hardware_interface::VelocityJointInterface velocity_joint_interface
Class for loading transmissions from a URDF description into ros_control interfaces.
hardware_interface::EffortJointInterface effort_joint_interface
Joint interfaces of a robot. Only used interfaces need to be populated.
Structs to hold tranmission data loaded straight from XML (URDF).
bool is_permutation(ForwardIt1 first, ForwardIt1 last, ForwardIt2 d_first)
static bool hasResource(const std::string &name, const Interface &iface)
hardware_interface::JointStateInterface joint_state_interface
pluginlib::ClassLoader< TransmissionLoader > TransmissionClassLoader
pluginlib::ClassLoader< RequisiteProvider > RequisiteProviderClassLoader
boost::shared_ptr< RequisiteProviderClassLoader > RequisiteProviderClassLoaderPtr
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:50
boost::shared_ptr< TransmissionClassLoader > TransmissionClassLoaderPtr
RobotTransmissions * robot_transmissions
Lifecycle is externally controlled (ie. hardware abstraction)
hardware_interface::PositionJointInterface position_joint_interface
std::map< std::string, RawJointData > RawJointDataMap


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Mon Apr 20 2020 03:52:15