transmission_interface_loader.cpp
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 
28 // C++ standard
29 #include <cassert>
30 #include <stdexcept>
31 
32 // ros_control
34 
35 
36 namespace transmission_interface
37 {
38 
40  TransmissionLoaderData& loader_data,
41  TransmissionSharedPtr transmission)
42 {
43  TransmissionHandleData handle_data;
44  handle_data.name = transmission_info.name_;
45  handle_data.transmission = transmission;
46 
47  // Check that required actuators are available in the robot
48  const bool act_state_data_ok = getActuatorStateData(transmission_info,
49  loader_data.robot_hw,
50  handle_data.act_state_data);
51  if (!act_state_data_ok) {return false;}
52 
53  const bool act_cmd_data_ok = getActuatorCommandData(transmission_info,
54  loader_data.robot_hw,
55  handle_data.act_cmd_data);
56  if (!act_cmd_data_ok) {return false;}
57 
58  // Update raw joint data
59  // This call potentially allocates resources in the raw data structure, so it should be the last requisite to setup.
60  // This is because we want to alter the state of the raw data only when we're sure that the transmission will load
61  // successfully
62  const bool jnt_data_ok = updateJointInterfaces(transmission_info,
63  loader_data.robot_hw,
64  loader_data.joint_interfaces,
65  loader_data.raw_joint_data_map);
66  if (!jnt_data_ok) {return false;}
67 
68  // Joint data
69  const bool jnt_state_data_ok = getJointStateData(transmission_info,
70  loader_data.raw_joint_data_map,
71  handle_data.jnt_state_data);
72  if (!jnt_state_data_ok) {return false;}
73 
74  const bool jnt_cmd_data_ok = getJointCommandData(transmission_info,
75  loader_data.raw_joint_data_map,
76  handle_data.jnt_cmd_data);
77  if (!jnt_cmd_data_ok) {return false;}
78 
79  // Update transmission interface
80  loader_data.transmission_data.push_back(transmission);
81 
82  // Register transmission
83  registerTransmission(loader_data, handle_data);
84 
85  return true;
86 }
87 
89  RobotTransmissions* robot_transmissions)
90  : robot_hw_ptr_(robot_hw),
91  robot_transmissions_ptr_(robot_transmissions)
92 {
93  // Can throw
94  transmission_class_loader_.reset(new TransmissionClassLoader("transmission_interface",
95  "transmission_interface::TransmissionLoader"));
96 
97  // Can throw
98  req_provider_loader_.reset(new RequisiteProviderClassLoader("transmission_interface",
99  "transmission_interface::RequisiteProvider"));
100 
101  // Throw if invalid
102  if (!robot_hw) {throw std::invalid_argument("Invalid robot hardware pointer.");}
103  if (!robot_transmissions) {throw std::invalid_argument("Invalid robot transmissions pointer.");}
104 
107 }
108 
109 bool TransmissionInterfaceLoader::load(const std::string& urdf)
110 {
111  TransmissionParser parser;
112  std::vector<TransmissionInfo> infos;
113  if (!parser.parse(urdf, infos)) {return false;}
114 
115  if (infos.empty())
116  {
117  ROS_ERROR_STREAM_NAMED("parser", "No transmissions were found in the robot description.");
118  return false;
119  }
120 
121  return load(infos);
122 }
123 
124 bool TransmissionInterfaceLoader::load(const std::vector<TransmissionInfo>& transmission_info_vec)
125 {
126  BOOST_FOREACH(const TransmissionInfo& info, transmission_info_vec)
127  {
128  if (!load(info)) {return false;}
129  }
130  return true;
131 }
132 
134 {
135  // Create transmission instance
136  TransmissionSharedPtr transmission;
137  try
138  {
139  TransmissionLoaderSharedPtr transmission_loader = transmission_class_loader_->createInstance(transmission_info.type_);
140  transmission = transmission_loader->load(transmission_info);
141  if (!transmission) {return false;}
142  }
144  {
145  ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
146  "'. Unsupported type '" << transmission_info.type_ << "'.\n" << ex.what());
147  return false;
148  }
149 
150  // We currently only deal with transmissions specifying a single hardware interface in the joints
151  assert(!transmission_info.joints_.empty() && !transmission_info.joints_.front().hardware_interfaces_.empty());
152  const std::vector<std::string>& hw_ifaces_ref = transmission_info.joints_.front().hardware_interfaces_; // First joint
153  BOOST_FOREACH(const JointInfo& jnt_info, transmission_info.joints_)
154  {
155  // Error out if at least one joint has a different set of hardware interfaces
156  if (hw_ifaces_ref.size() != jnt_info.hardware_interfaces_.size() ||
157  !internal::is_permutation(hw_ifaces_ref.begin(), hw_ifaces_ref.end(),
158  jnt_info.hardware_interfaces_.begin()))
159  {
160  ROS_ERROR_STREAM_NAMED("parser",
161  "Failed to load transmission '" << transmission_info.name_ <<
162  "'. It has joints with different hardware interfaces. This is currently unsupported.");
163  return false;
164  }
165  }
166 
167  // Load transmission for all specified hardware interfaces
168  bool has_at_least_one_hw_iface = false;
169  BOOST_FOREACH(const std::string& hw_iface, hw_ifaces_ref)
170  {
171  RequisiteProviderPtr req_provider;
172  try
173  {
174  req_provider = req_provider_loader_->createInstance(hw_iface);
175  if (!req_provider) {continue;}
176  }
178  {
179  ROS_WARN_STREAM_NAMED("parser", "Failed to process the '" << hw_iface <<
180  "' hardware interface for transmission '" << transmission_info.name_ <<
181  "'.\n" << ex.what());
182  continue;
183  }
184 
185  const bool load_ok = req_provider->loadTransmissionMaps(transmission_info, loader_data_, transmission);
186  if (load_ok) {has_at_least_one_hw_iface = true;}
187  else {continue;}
188  }
189 
190  if (!has_at_least_one_hw_iface)
191  {
192  ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
193  "'. It contains no valid hardware interfaces.");
194  return false;
195  }
196 
197  return true;
198 }
199 
200 } // namespace
static bool parse(const std::string &urdf_string, std::vector< TransmissionInfo > &transmissions)
Parses the tranmission elements of a URDF.
hardware_interface::RobotHW * robot_hw
Lifecycle is externally controlled (ie. hardware abstraction)
Parse all transmissions specified in a URDF.
#define ROS_ERROR_STREAM_NAMED(name, args)
TransmissionInterfaceLoader(hardware_interface::RobotHW *robot_hw, RobotTransmissions *robot_transmissions)
Contains semantic info about a given joint loaded from XML (URDF)
Contains semantic info about a given transmission loaded from XML (URDF)
bool load(const std::string &urdf)
Load all transmissions in a URDF.
virtual bool updateJointInterfaces(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, JointInterfaces &joint_interfaces, RawJointDataMap &raw_joint_data_map)=0
Update a robot&#39;s joint interfaces with joint information contained in a transmission.
virtual bool getActuatorCommandData(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, ActuatorData &act_cmd_data)=0
bool is_permutation(ForwardIt1 first, ForwardIt1 last, ForwardIt2 d_first)
virtual bool getActuatorStateData(const TransmissionInfo &transmission_info, hardware_interface::RobotHW *robot_hw, ActuatorData &act_state_data)=0
pluginlib::ClassLoader< TransmissionLoader > TransmissionClassLoader
pluginlib::ClassLoader< RequisiteProvider > RequisiteProviderClassLoader
virtual bool registerTransmission(TransmissionLoaderData &loader_data, TransmissionHandleData &handle_data)=0
RobotTransmissions * robot_transmissions
Lifecycle is externally controlled (ie. hardware abstraction)
virtual bool getJointStateData(const TransmissionInfo &transmission_info, const RawJointDataMap &raw_joint_data_map, JointData &jnt_state_data)=0
bool loadTransmissionMaps(const TransmissionInfo &transmission_info, TransmissionLoaderData &loader_data, TransmissionSharedPtr transmission)
std::vector< std::string > hardware_interfaces_
virtual bool getJointCommandData(const TransmissionInfo &transmission_info, const RawJointDataMap &raw_joint_data_map, JointData &jnt_cmd_data)=0
#define ROS_WARN_STREAM_NAMED(name, args)


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