simple_transmission_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 // ROS
29 #include <ros/console.h>
30 
31 // Pluginlib
33 
34 // ros_control
38 
39 namespace transmission_interface
40 {
41 
43 {
44  // Transmission should contain only one actuator/joint
45  if (!checkActuatorDimension(transmission_info, 1)) {return TransmissionSharedPtr();}
46  if (!checkJointDimension(transmission_info, 1)) {return TransmissionSharedPtr();}
47 
48  // Parse actuator and joint xml elements
49  TiXmlElement actuator_el = loadXmlElement(transmission_info.actuators_.front().xml_element_);
50  TiXmlElement joint_el = loadXmlElement(transmission_info.joints_.front().xml_element_);
51 
52  // Parse required mechanical reduction
53  double reduction = 0.0;
54  const ParseStatus reduction_status = getActuatorReduction(actuator_el,
55  transmission_info.actuators_.front().name_,
56  transmission_info.name_,
57  true, // Required
58  reduction);
59  if (reduction_status != SUCCESS) {return TransmissionSharedPtr();}
60 
61  // Parse optional joint offset. Even though it's optional --and to avoid surprises-- we fail if the element is
62  // specified but is of the wrong type
63  double joint_offset = 0.0;
64  const ParseStatus joint_offset_status = getJointOffset(joint_el,
65  transmission_info.joints_.front().name_,
66  transmission_info.name_,
67  false, // Optional
68  joint_offset);
69  if (joint_offset_status == BAD_TYPE) {return TransmissionSharedPtr();}
70 
71  // Transmission instance
72  try
73  {
74  TransmissionSharedPtr transmission(new SimpleTransmission(reduction, joint_offset));
75  return transmission;
76  }
77  catch(const TransmissionInterfaceException& ex)
78  {
80  ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" << transmission_info.name_ << "' of type '" <<
81  demangledTypeName<SimpleTransmission>()<< "'. " << ex.what());
82  return TransmissionSharedPtr();
83  }
84 }
85 
86 } // namespace
87 
static TiXmlElement loadXmlElement(const std::string &element_str)
#define ROS_ERROR_STREAM_NAMED(name, args)
Abstract interface for loading transmission instances from configuration data.
Contains semantic info about a given transmission loaded from XML (URDF)
boost::shared_ptr< Transmission > TransmissionSharedPtr
Definition: transmission.h:165
static ParseStatus getActuatorReduction(const TiXmlElement &parent_el, const std::string &actuator_name, const std::string &transmission_name, bool required, double &reduction)
Class for loading a simple transmission instance from configuration data.
static bool checkActuatorDimension(const TransmissionInfo &transmission_info, const unsigned int expected_dim)
Implementation of a simple reducer transmission.
std::vector< ActuatorInfo > actuators_
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
TransmissionSharedPtr load(const TransmissionInfo &transmission_info)
static ParseStatus getJointOffset(const TiXmlElement &parent_el, const std::string &joint_name, const std::string &transmission_name, bool required, double &offset)
static bool checkJointDimension(const TransmissionInfo &transmission_info, const unsigned int expected_dim)


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