differential_transmission_loader.cpp
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics S.L. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00028 // ROS
00029 #include <ros/console.h>
00030 
00031 // Pluginlib
00032 #include <pluginlib/class_list_macros.h>
00033 
00034 // ros_control
00035 #include <hardware_interface/internal/demangle_symbol.h>
00036 #include <transmission_interface/differential_transmission.h>
00037 #include <transmission_interface/differential_transmission_loader.h>
00038 
00039 namespace transmission_interface
00040 {
00041 
00042 DifferentialTransmissionLoader::TransmissionPtr
00043 DifferentialTransmissionLoader::load(const TransmissionInfo& transmission_info)
00044 {
00045   // Transmission should contain only one actuator/joint
00046   if (!checkActuatorDimension(transmission_info, 2)) {return TransmissionPtr();}
00047   if (!checkJointDimension(transmission_info,    2)) {return TransmissionPtr();}
00048 
00049   // Get actuator and joint configuration sorted by role: [actuator1, actuator2] and [joint1, joint2]
00050   std::vector<double> act_reduction;
00051   const bool act_config_ok = getActuatorConfig(transmission_info, act_reduction);
00052   if (!act_config_ok) {return TransmissionPtr();}
00053 
00054   std::vector<double> jnt_reduction;
00055   std::vector<double> jnt_offset;
00056   const bool jnt_config_ok = getJointConfig(transmission_info,
00057                                             jnt_reduction,
00058                                             jnt_offset);
00059 
00060   if (!jnt_config_ok) {return TransmissionPtr();}
00061 
00062   // Transmission instance
00063   try
00064   {
00065     TransmissionPtr transmission(new DifferentialTransmission(act_reduction,
00066                                                               jnt_reduction,
00067                                                               jnt_offset));
00068     return transmission;
00069   }
00070   catch(const TransmissionInterfaceException& ex)
00071   {
00072     using hardware_interface::internal::demangledTypeName;
00073     ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" << transmission_info.name_ << "' of type '" <<
00074                            demangledTypeName<DifferentialTransmission>()<< "'. " << ex.what());
00075     return TransmissionPtr();
00076   }
00077 }
00078 
00079 bool DifferentialTransmissionLoader::getActuatorConfig(const TransmissionInfo& transmission_info,
00080                                                        std::vector<double>&    actuator_reduction)
00081 {
00082   const std::string ACTUATOR1_ROLE = "actuator1";
00083   const std::string ACTUATOR2_ROLE = "actuator2";
00084 
00085   std::vector<TiXmlElement> act_elements(2,"");
00086   std::vector<std::string>  act_names(2);
00087   std::vector<std::string>  act_roles(2);
00088 
00089   for (unsigned int i = 0; i < 2; ++i)
00090   {
00091     // Actuator name
00092     act_names[i] = transmission_info.actuators_[i].name_;
00093 
00094     // Actuator xml element
00095     act_elements[i] = loadXmlElement(transmission_info.actuators_[i].xml_element_);
00096 
00097     // Populate role string
00098     std::string& act_role = act_roles[i];
00099     const ParseStatus act_role_status = getActuatorRole(act_elements[i],
00100                                                         act_names[i],
00101                                                         transmission_info.name_,
00102                                                         true, // Required
00103                                                         act_role);
00104     if (act_role_status != SUCCESS) {return false;}
00105 
00106     // Validate role string
00107     if (ACTUATOR1_ROLE != act_role && ACTUATOR2_ROLE != act_role)
00108     {
00109       ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << act_names[i] << "' of transmission '" << transmission_info.name_ <<
00110                              "' does not specify a valid <role> element. Got '" << act_role << "', expected '" <<
00111                              ACTUATOR1_ROLE << "' or '" << ACTUATOR2_ROLE << "'.");
00112       return false;
00113     }
00114   }
00115 
00116   // Roles must be different
00117   if (act_roles[0] == act_roles[1])
00118   {
00119     ROS_ERROR_STREAM_NAMED("parser", "Actuators '" << act_names[0] << "' and '" << act_names[1] <<
00120                            "' of transmission '" << transmission_info.name_ <<
00121                            "' must have different roles. Both specify '" << act_roles[0] << "'.");
00122     return false;
00123   }
00124 
00125   // Indices sorted according to role
00126   std::vector<unsigned int> id_map(2);
00127   if (ACTUATOR1_ROLE == act_roles[0])
00128   {
00129     id_map[0] = 0;
00130     id_map[1] = 1;
00131 
00132   }
00133   else
00134   {
00135     id_map[0] = 1;
00136     id_map[1] = 0;
00137   }
00138 
00139   // Parse required mechanical reductions
00140   actuator_reduction.resize(2);
00141   for (unsigned int i = 0; i < 2; ++i)
00142   {
00143     const unsigned int id = id_map[i];
00144     const ParseStatus reduction_status = getActuatorReduction(act_elements[id],
00145                                                               act_names[id],
00146                                                               transmission_info.name_,
00147                                                               true, // Required
00148                                                               actuator_reduction[i]);
00149     if (reduction_status != SUCCESS) {return false;}
00150   }
00151 
00152   return true;
00153 }
00154 
00155 bool DifferentialTransmissionLoader::getJointConfig(const TransmissionInfo& transmission_info,
00156                                                     std::vector<double>&    joint_reduction,
00157                                                     std::vector<double>&    joint_offset)
00158 {
00159   const std::string JOINT1_ROLE = "joint1";
00160   const std::string JOINT2_ROLE = "joint2";
00161 
00162   std::vector<TiXmlElement> jnt_elements(2,"");
00163   std::vector<std::string>  jnt_names(2);
00164   std::vector<std::string>  jnt_roles(2);
00165 
00166   for (unsigned int i = 0; i < 2; ++i)
00167   {
00168     // Joint name
00169     jnt_names[i] = transmission_info.joints_[i].name_;
00170 
00171     // Joint xml element
00172     jnt_elements[i] = loadXmlElement(transmission_info.joints_[i].xml_element_);
00173 
00174     // Populate role string
00175     std::string& jnt_role = jnt_roles[i];
00176     const ParseStatus jnt_role_status = getJointRole(jnt_elements[i],
00177                                                      jnt_names[i],
00178                                                      transmission_info.name_,
00179                                                      true, // Required
00180                                                      jnt_role);
00181     if (jnt_role_status != SUCCESS) {return false;}
00182 
00183     // Validate role string
00184     if (JOINT1_ROLE != jnt_role && JOINT2_ROLE != jnt_role)
00185     {
00186       ROS_ERROR_STREAM_NAMED("parser", "Joint '" << jnt_names[i] << "' of transmission '" << transmission_info.name_ <<
00187                              "' does not specify a valid <role> element. Got '" << jnt_role << "', expected '" <<
00188                              JOINT1_ROLE << "' or '" << JOINT2_ROLE << "'.");
00189       return false;
00190     }
00191   }
00192 
00193   // Roles must be different
00194   if (jnt_roles[0] == jnt_roles[1])
00195   {
00196     ROS_ERROR_STREAM_NAMED("parser", "Joints '" << jnt_names[0] << "' and '" << jnt_names[1] <<
00197                            "' of transmission '" << transmission_info.name_ <<
00198                            "' must have different roles. Both specify '" << jnt_roles[0] << "'.");
00199     return false;
00200   }
00201 
00202   // Indices sorted according to role
00203   std::vector<unsigned int> id_map(2);
00204   if (JOINT1_ROLE == jnt_roles[0])
00205   {
00206     id_map[0] = 0;
00207     id_map[1] = 1;
00208 
00209   }
00210   else
00211   {
00212     id_map[0] = 1;
00213     id_map[1] = 0;
00214   }
00215 
00216   // Joint configuration
00217   joint_reduction.resize(2, 1.0);
00218   joint_offset.resize(2, 0.0);
00219   for (unsigned int i = 0; i < 2; ++i)
00220   {
00221     const unsigned int id = id_map[i];
00222 
00223     // Parse optional mechanical reductions. Even though it's optional --and to avoid surprises-- we fail if the element
00224     // is specified but is of the wrong type
00225     const ParseStatus reduction_status = getJointReduction(jnt_elements[id],
00226                                                            jnt_names[id],
00227                                                            transmission_info.name_,
00228                                                            false, // Optional
00229                                                            joint_reduction[i]);
00230     if (reduction_status == BAD_TYPE) {return false;}
00231 
00232     // Parse optional joint offset. Even though it's optional --and to avoid surprises-- we fail if the element is
00233     // specified but is of the wrong type
00234     const ParseStatus offset_status = getJointOffset(jnt_elements[id],
00235                                                      jnt_names[id],
00236                                                      transmission_info.name_,
00237                                                      false, // Optional
00238                                                      joint_offset[i]);
00239     if (offset_status == BAD_TYPE) {return false;}
00240   }
00241 
00242   return true;
00243 }
00244 
00245 } // namespace
00246 
00247 PLUGINLIB_EXPORT_CLASS(transmission_interface::DifferentialTransmissionLoader,
00248                        transmission_interface::TransmissionLoader)


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Thu Dec 1 2016 03:46:10