transmission_interface_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 // C++ standard
00029 #include <cassert>
00030 #include <stdexcept>
00031 
00032 // ros_control
00033 #include <transmission_interface/transmission_interface_loader.h>
00034 
00035 
00036 namespace transmission_interface
00037 {
00038 
00039 bool RequisiteProvider::loadTransmissionMaps(const TransmissionInfo& transmission_info,
00040                                              TransmissionLoaderData& loader_data,
00041                                              TransmissionPtr         transmission)
00042 {
00043   TransmissionHandleData handle_data;
00044   handle_data.name         = transmission_info.name_;
00045   handle_data.transmission = transmission;
00046 
00047   // Check that required actuators are available in the robot
00048   const bool act_state_data_ok = getActuatorStateData(transmission_info,
00049                                                       loader_data.robot_hw,
00050                                                       handle_data.act_state_data);
00051   if (!act_state_data_ok) {return false;}
00052 
00053   const bool act_cmd_data_ok = getActuatorCommandData(transmission_info,
00054                                                       loader_data.robot_hw,
00055                                                       handle_data.act_cmd_data);
00056   if (!act_cmd_data_ok) {return false;}
00057 
00058   // Update raw joint data
00059   // This call potentially allocates resources in the raw data structure, so it should be the last requisite to setup.
00060   // This is because we want to alter the state of the raw data only when we're sure that the transmission will load
00061   // successfully
00062   const bool jnt_data_ok = updateJointInterfaces(transmission_info,
00063                                                  loader_data.robot_hw,
00064                                                  loader_data.joint_interfaces,
00065                                                  loader_data.raw_joint_data_map);
00066   if (!jnt_data_ok) {return false;}
00067 
00068   // Joint data
00069   const bool jnt_state_data_ok = getJointStateData(transmission_info,
00070                                                    loader_data.raw_joint_data_map,
00071                                                    handle_data.jnt_state_data);
00072   if (!jnt_state_data_ok) {return false;}
00073 
00074   const bool jnt_cmd_data_ok = getJointCommandData(transmission_info,
00075                                                    loader_data.raw_joint_data_map,
00076                                                    handle_data.jnt_cmd_data);
00077   if (!jnt_cmd_data_ok) {return false;}
00078 
00079   // Update transmission interface
00080   loader_data.transmission_data.push_back(transmission);
00081 
00082   // Register transmission
00083   registerTransmission(loader_data, handle_data);
00084 
00085   return true;
00086 }
00087 
00088 TransmissionInterfaceLoader::TransmissionInterfaceLoader(hardware_interface::RobotHW* robot_hw,
00089                                                          RobotTransmissions*          robot_transmissions)
00090   : robot_hw_ptr_(robot_hw),
00091     robot_transmissions_ptr_(robot_transmissions)
00092 {
00093   // Can throw
00094   transmission_class_loader_.reset(new TransmissionClassLoader("transmission_interface",
00095                                                                "transmission_interface::TransmissionLoader"));
00096 
00097   // Can throw
00098   req_provider_loader_.reset(new RequisiteProviderClassLoader("transmission_interface",
00099                                                               "transmission_interface::RequisiteProvider"));
00100 
00101   // Throw if invalid
00102   if (!robot_hw)            {throw std::invalid_argument("Invalid robot hardware pointer.");}
00103   if (!robot_transmissions) {throw std::invalid_argument("Invalid robot transmissions pointer.");}
00104 
00105   loader_data_.robot_hw            =  robot_hw_ptr_;
00106   loader_data_.robot_transmissions =  robot_transmissions_ptr_;
00107 }
00108 
00109 bool TransmissionInterfaceLoader::load(const std::string& urdf)
00110 {
00111   TransmissionParser parser;
00112   std::vector<TransmissionInfo> infos;
00113   if (!parser.parse(urdf, infos)) {return false;}
00114 
00115   if (infos.empty())
00116   {
00117     ROS_ERROR_STREAM_NAMED("parser", "No transmissions were found in the robot description.");
00118     return false;
00119   }
00120 
00121   return load(infos);
00122 }
00123 
00124 bool TransmissionInterfaceLoader::load(const std::vector<TransmissionInfo>& transmission_info_vec)
00125 {
00126   BOOST_FOREACH(const TransmissionInfo& info, transmission_info_vec)
00127   {
00128     if (!load(info)) {return false;}
00129   }
00130   return true;
00131 }
00132 
00133 bool TransmissionInterfaceLoader::load(const TransmissionInfo& transmission_info)
00134 {
00135   // Create transmission instance
00136   TransmissionPtr transmission;
00137   try
00138   {
00139     TransmissionLoaderPtr transmission_loader = transmission_class_loader_->createInstance(transmission_info.type_);
00140     transmission = transmission_loader->load(transmission_info);
00141     if (!transmission) {return false;}
00142   }
00143   catch(pluginlib::LibraryLoadException &ex)
00144   {
00145     ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
00146                            "'. Unsupported type '" << transmission_info.type_ << "'.\n" << ex.what());
00147     return false;
00148   }
00149 
00150   // We currently only deal with transmissions specifying a single hardware interface in the joints
00151   assert(!transmission_info.joints_.empty() && !transmission_info.joints_.front().hardware_interfaces_.empty());
00152   const std::vector<std::string>& hw_ifaces_ref = transmission_info.joints_.front().hardware_interfaces_; // First joint
00153   BOOST_FOREACH(const JointInfo& jnt_info, transmission_info.joints_)
00154   {
00155     // Error out if at least one joint has a different set of hardware interfaces
00156     if (hw_ifaces_ref.size() != jnt_info.hardware_interfaces_.size() ||
00157         !internal::is_permutation(hw_ifaces_ref.begin(), hw_ifaces_ref.end(),
00158                                   jnt_info.hardware_interfaces_.begin()))
00159     {
00160       ROS_ERROR_STREAM_NAMED("parser",
00161                              "Failed to load transmission '" << transmission_info.name_ <<
00162                              "'. It has joints with different hardware interfaces. This is currently unsupported.");
00163       return false;
00164     }
00165   }
00166 
00167   // Load transmission for all specified hardware interfaces
00168   bool has_at_least_one_hw_iface = false;
00169   BOOST_FOREACH(const std::string& hw_iface, hw_ifaces_ref)
00170   {
00171     RequisiteProviderPtr req_provider;
00172     try
00173     {
00174       req_provider = req_provider_loader_->createInstance(hw_iface);
00175       if (!req_provider) {continue;}
00176     }
00177     catch(pluginlib::LibraryLoadException &ex)
00178     {
00179       ROS_WARN_STREAM_NAMED("parser", "Failed to process the '" << hw_iface <<
00180                              "' hardware interface for transmission '" << transmission_info.name_ <<
00181                              "'.\n" << ex.what());
00182       continue;
00183     }
00184 
00185     const bool load_ok = req_provider->loadTransmissionMaps(transmission_info, loader_data_, transmission);
00186     if (load_ok) {has_at_least_one_hw_iface = true;}
00187     else {continue;}
00188   }
00189 
00190   if (!has_at_least_one_hw_iface)
00191   {
00192     ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
00193                            "'. It contains no valid hardware interfaces.");
00194     return false;
00195   }
00196 
00197   return true;
00198 }
00199 
00200 } // namespace


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