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