kinematics_plugin_loader.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Dave Coleman */
00036 
00037 #include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h>
00038 #include <moveit/rdf_loader/rdf_loader.h>
00039 #include <pluginlib/class_loader.h>
00040 #include <boost/thread/mutex.hpp>
00041 #include <sstream>
00042 #include <vector>
00043 #include <map>
00044 #include <ros/ros.h>
00045 #include <moveit/profiler/profiler.h>
00046 
00047 namespace kinematics_plugin_loader
00048 {
00049 class KinematicsPluginLoader::KinematicsLoaderImpl
00050 {
00051 public:
00059   KinematicsLoaderImpl(const std::string& robot_description,
00060                        const std::map<std::string, std::vector<std::string> >& possible_kinematics_solvers,
00061                        const std::map<std::string, std::vector<double> >& search_res,
00062                        const std::map<std::string, std::vector<std::string> >& iksolver_to_tip_links)
00063     : robot_description_(robot_description)
00064     , possible_kinematics_solvers_(possible_kinematics_solvers)
00065     , search_res_(search_res)
00066     , iksolver_to_tip_links_(iksolver_to_tip_links)
00067   {
00068     try
00069     {
00070       kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::"
00071                                                                                                      "KinematicsBase"));
00072     }
00073     catch (pluginlib::PluginlibException& e)
00074     {
00075       ROS_ERROR("Unable to construct kinematics loader. Error: %s", e.what());
00076     }
00077   }
00078 
00084   std::vector<std::string> chooseTipFrames(const robot_model::JointModelGroup* jmg)
00085   {
00086     std::vector<std::string> tips;
00087     std::map<std::string, std::vector<std::string> >::const_iterator ik_it =
00088         iksolver_to_tip_links_.find(jmg->getName());
00089 
00090     // Check if tips were loaded onto the rosparam server previously
00091     if (ik_it != iksolver_to_tip_links_.end())
00092     {
00093       // the tip is being chosen based on a corresponding rosparam ik link
00094       ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Chooing tip frame of kinematic solver for group "
00095                                                              << jmg->getName()
00096                                                              << " based on values in rosparam server.");
00097       tips = ik_it->second;
00098     }
00099     else
00100     {
00101       // get the last link in the chain
00102       ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Chooing tip frame of kinematic solver for group "
00103                                                              << jmg->getName() << " based on last link in chain");
00104 
00105       tips.push_back(jmg->getLinkModels().back()->getName());
00106     }
00107 
00108     // Error check
00109     if (tips.empty())
00110     {
00111       ROS_ERROR_STREAM_NAMED("kinematics_plugin_loader", "Error choosing kinematic solver tip frame(s).");
00112     }
00113 
00114     // Debug tip choices
00115     std::stringstream tip_debug;
00116     tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): ";
00117     for (std::size_t i = 0; i < tips.size(); ++i)
00118       tip_debug << tips[i] << ", ";
00119     ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", tip_debug.str());
00120 
00121     return tips;
00122   }
00123 
00124   kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup* jmg)
00125   {
00126     kinematics::KinematicsBasePtr result;
00127     if (!jmg)
00128     {
00129       ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
00130       return result;
00131     }
00132 
00133     ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());
00134 
00135     if (kinematics_loader_ && jmg)
00136     {
00137       std::map<std::string, std::vector<std::string> >::const_iterator it =
00138           possible_kinematics_solvers_.find(jmg->getName());
00139       if (it != possible_kinematics_solvers_.end())
00140       {
00141         // just to be sure, do not call the same pluginlib instance allocation function in parallel
00142         boost::mutex::scoped_lock slock(lock_);
00143 
00144         for (std::size_t i = 0; !result && i < it->second.size(); ++i)
00145         {
00146           try
00147           {
00148             result = kinematics_loader_->createInstance(it->second[i]);
00149             if (result)
00150             {
00151               const std::vector<const robot_model::LinkModel*>& links = jmg->getLinkModels();
00152               if (!links.empty())
00153               {
00154                 const std::string& base = links.front()->getParentJointModel()->getParentLinkModel() ?
00155                                               links.front()->getParentJointModel()->getParentLinkModel()->getName() :
00156                                               jmg->getParentModel().getModelFrame();
00157 
00158                 // choose the tip of the IK solver
00159                 const std::vector<std::string> tips = chooseTipFrames(jmg);
00160 
00161                 // choose search resolution
00162                 double search_res =
00163                     search_res_.find(jmg->getName())->second[i];  // we know this exists, by construction
00164 
00165                 if (!result->initialize(robot_description_, jmg->getName(),
00166                                         (base.empty() || base[0] != '/') ? base : base.substr(1), tips, search_res))
00167                 {
00168                   ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'",
00169                             it->second[i].c_str(), jmg->getName().c_str());
00170                   result.reset();
00171                 }
00172                 else
00173                 {
00174                   result->setDefaultTimeout(jmg->getDefaultIKTimeout());
00175                   ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search "
00176                             "resolution %lf for group '%s' at address %p",
00177                             it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
00178                 }
00179               }
00180               else
00181                 ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
00182             }
00183           }
00184           catch (pluginlib::PluginlibException& e)
00185           {
00186             ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
00187           }
00188         }
00189       }
00190       else
00191         ROS_DEBUG("No kinematics solver available for this group");
00192     }
00193 
00194     if (!result)
00195     {
00196       ROS_DEBUG("No usable kinematics solver was found for this group.");
00197       ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
00198     }
00199     return result;
00200   }
00201 
00202   kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup* jmg)
00203   {
00204     {
00205       boost::mutex::scoped_lock slock(lock_);
00206       const std::vector<kinematics::KinematicsBasePtr>& vi = instances_[jmg];
00207       for (std::size_t i = 0; i < vi.size(); ++i)
00208         if (vi[i].unique())
00209         {
00210           ROS_DEBUG("Reusing cached kinematics solver for group '%s'", jmg->getName().c_str());
00211           return vi[i];  // this is safe since the shared_ptr is copied on stack BEFORE the destructors in scope get
00212                          // called
00213         }
00214     }
00215 
00216     kinematics::KinematicsBasePtr res = allocKinematicsSolver(jmg);
00217 
00218     {
00219       boost::mutex::scoped_lock slock(lock_);
00220       instances_[jmg].push_back(res);
00221       return res;
00222     }
00223   }
00224 
00225   void status() const
00226   {
00227     for (std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.begin();
00228          it != possible_kinematics_solvers_.end(); ++it)
00229       for (std::size_t i = 0; i < it->second.size(); ++i)
00230         ROS_INFO("Solver for group '%s': '%s' (search resolution = %lf)", it->first.c_str(), it->second[i].c_str(),
00231                  search_res_.at(it->first)[i]);
00232   }
00233 
00234 private:
00235   std::string robot_description_;
00236   std::map<std::string, std::vector<std::string> > possible_kinematics_solvers_;
00237   std::map<std::string, std::vector<double> > search_res_;
00238   std::map<std::string, std::vector<std::string> > iksolver_to_tip_links_;  // a map between each ik solver and a vector
00239                                                                             // of custom-specified tip link(s)
00240   boost::shared_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase> > kinematics_loader_;
00241   std::map<const robot_model::JointModelGroup*, std::vector<kinematics::KinematicsBasePtr> > instances_;
00242   boost::mutex lock_;
00243 };
00244 }
00245 
00246 void kinematics_plugin_loader::KinematicsPluginLoader::status() const
00247 {
00248   if (loader_)
00249     loader_->status();
00250   else
00251     ROS_INFO("Loader function was never required");
00252 }
00253 
00254 robot_model::SolverAllocatorFn kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction()
00255 {
00256   moveit::tools::Profiler::ScopedStart prof_start;
00257   moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction");
00258 
00259   if (loader_)
00260     return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(),
00261                        _1);
00262 
00263   rdf_loader::RDFLoader rml(robot_description_);
00264   robot_description_ = rml.getRobotDescription();
00265   return getLoaderFunction(rml.getSRDF());
00266 }
00267 
00268 robot_model::SolverAllocatorFn
00269 kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction(const boost::shared_ptr<srdf::Model>& srdf_model)
00270 {
00271   moveit::tools::Profiler::ScopedStart prof_start;
00272   moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)");
00273 
00274   if (!loader_)
00275   {
00276     ROS_DEBUG("Configuring kinematics solvers");
00277     groups_.clear();
00278 
00279     std::map<std::string, std::vector<std::string> > possible_kinematics_solvers;
00280     std::map<std::string, std::vector<double> > search_res;
00281     std::map<std::string, std::vector<std::string> > iksolver_to_tip_links;
00282 
00283     if (srdf_model)
00284     {
00285       const std::vector<srdf::Model::Group>& known_groups = srdf_model->getGroups();
00286       if (default_search_resolution_ <= std::numeric_limits<double>::epsilon())
00287         default_search_resolution_ = kinematics::KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION;
00288 
00289       if (default_solver_plugin_.empty())
00290       {
00291         ROS_DEBUG("Loading settings for kinematics solvers from the ROS param server ...");
00292 
00293         // read data using ROS params
00294         ros::NodeHandle nh("~");
00295 
00296         // read the list of plugin names for possible kinematics solvers
00297         for (std::size_t i = 0; i < known_groups.size(); ++i)
00298         {
00299           std::string base_param_name = known_groups[i].name_;
00300           ROS_DEBUG_NAMED("kinematics_plugin_loader", "Looking for param %s ",
00301                           (base_param_name + "/kinematics_solver").c_str());
00302           std::string ksolver_param_name;
00303           bool found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
00304           if (!found || !nh.hasParam(ksolver_param_name))
00305           {
00306             base_param_name = robot_description_ + "_kinematics/" + known_groups[i].name_;
00307             ROS_DEBUG_NAMED("kinematics_plugin_loader", "Looking for param %s ",
00308                             (base_param_name + "/kinematics_solver").c_str());
00309             found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
00310           }
00311           if (found)
00312           {
00313             ROS_DEBUG_NAMED("kinematics_plugin_loader", "Found param %s", ksolver_param_name.c_str());
00314             std::string ksolver;
00315             if (nh.getParam(ksolver_param_name, ksolver))
00316             {
00317               std::stringstream ss(ksolver);
00318               bool first = true;
00319               while (ss.good() && !ss.eof())
00320               {
00321                 if (first)
00322                 {
00323                   first = false;
00324                   groups_.push_back(known_groups[i].name_);
00325                 }
00326                 std::string solver;
00327                 ss >> solver >> std::ws;
00328                 possible_kinematics_solvers[known_groups[i].name_].push_back(solver);
00329                 ROS_DEBUG_NAMED("kinematics_plugin_loader", "Using kinematics solver '%s' for group '%s'.",
00330                                 solver.c_str(), known_groups[i].name_.c_str());
00331               }
00332             }
00333           }
00334 
00335           std::string ksolver_timeout_param_name;
00336           if (nh.searchParam(base_param_name + "/kinematics_solver_timeout", ksolver_timeout_param_name))
00337           {
00338             double ksolver_timeout;
00339             if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout))
00340               ik_timeout_[known_groups[i].name_] = ksolver_timeout;
00341             else
00342             {  // just in case this is an int
00343               int ksolver_timeout_i;
00344               if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout_i))
00345                 ik_timeout_[known_groups[i].name_] = ksolver_timeout_i;
00346             }
00347           }
00348 
00349           std::string ksolver_attempts_param_name;
00350           if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name))
00351           {
00352             int ksolver_attempts;
00353             if (nh.getParam(ksolver_attempts_param_name, ksolver_attempts))
00354               ik_attempts_[known_groups[i].name_] = ksolver_attempts;
00355           }
00356 
00357           std::string ksolver_res_param_name;
00358           if (nh.searchParam(base_param_name + "/kinematics_solver_search_resolution", ksolver_res_param_name))
00359           {
00360             std::string ksolver_res;
00361             if (nh.getParam(ksolver_res_param_name, ksolver_res))
00362             {
00363               std::stringstream ss(ksolver_res);
00364               while (ss.good() && !ss.eof())
00365               {
00366                 double res;
00367                 ss >> res >> std::ws;
00368                 search_res[known_groups[i].name_].push_back(res);
00369               }
00370             }
00371             else
00372             {  // handle the case this param is just one value and parsed as a double
00373               double res;
00374               if (nh.getParam(ksolver_res_param_name, res))
00375                 search_res[known_groups[i].name_].push_back(res);
00376               else
00377               {
00378                 int res_i;
00379                 if (nh.getParam(ksolver_res_param_name, res_i))
00380                   search_res[known_groups[i].name_].push_back(res_i);
00381               }
00382             }
00383           }
00384 
00385           // Allow a kinematic solver's tip link to be specified on the rosparam server
00386           // Depreciated in favor of array version now
00387           std::string ksolver_ik_link_param_name;
00388           if (nh.searchParam(base_param_name + "/kinematics_solver_ik_link", ksolver_ik_link_param_name))
00389           {
00390             std::string ksolver_ik_link;
00391             if (nh.getParam(ksolver_ik_link_param_name, ksolver_ik_link))  // has a custom rosparam-based tip link
00392             {
00393               ROS_WARN_STREAM_NAMED("kinematics_plugin_loader", "Using kinematics_solver_ik_link rosparam is "
00394                                                                 "deprecated in favor of kinematics_solver_ik_links "
00395                                                                 "rosparam array.");
00396               iksolver_to_tip_links[known_groups[i].name_].push_back(ksolver_ik_link);
00397             }
00398           }
00399 
00400           // Allow a kinematic solver's tip links to be specified on the rosparam server as an array
00401           std::string ksolver_ik_links_param_name;
00402           if (nh.searchParam(base_param_name + "/kinematics_solver_ik_links", ksolver_ik_links_param_name))
00403           {
00404             XmlRpc::XmlRpcValue ksolver_ik_links;
00405             if (nh.getParam(ksolver_ik_links_param_name, ksolver_ik_links))  // has custom rosparam-based tip link(s)
00406             {
00407               if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray)
00408               {
00409                 ROS_WARN_STREAM_NAMED("kinematics_plugin_loader", "rosparam '"
00410                                                                       << ksolver_ik_links_param_name
00411                                                                       << "' should be an XmlRpc value type 'Array'");
00412               }
00413               else
00414               {
00415                 for (int32_t j = 0; j < ksolver_ik_links.size(); ++j)
00416                 {
00417                   ROS_ASSERT(ksolver_ik_links[j].getType() == XmlRpc::XmlRpcValue::TypeString);
00418                   ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader",
00419                                          "found tip " << static_cast<std::string>(ksolver_ik_links[j]) << " for group "
00420                                                       << known_groups[i].name_);
00421                   iksolver_to_tip_links[known_groups[i].name_].push_back(static_cast<std::string>(ksolver_ik_links[j]));
00422                 }
00423               }
00424             }
00425           }
00426 
00427           // make sure there is a default resolution at least specified for every solver (in case it was not specified
00428           // on the param server)
00429           while (search_res[known_groups[i].name_].size() < possible_kinematics_solvers[known_groups[i].name_].size())
00430             search_res[known_groups[i].name_].push_back(default_search_resolution_);
00431         }
00432       }
00433       else
00434       {
00435         ROS_DEBUG("Using specified default settings for kinematics solvers ...");
00436         for (std::size_t i = 0; i < known_groups.size(); ++i)
00437         {
00438           possible_kinematics_solvers[known_groups[i].name_].resize(1, default_solver_plugin_);
00439           search_res[known_groups[i].name_].resize(1, default_search_resolution_);
00440           ik_timeout_[known_groups[i].name_] = default_solver_timeout_;
00441           ik_attempts_[known_groups[i].name_] = default_ik_attempts_;
00442           groups_.push_back(known_groups[i].name_);
00443         }
00444       }
00445     }
00446 
00447     loader_.reset(
00448         new KinematicsLoaderImpl(robot_description_, possible_kinematics_solvers, search_res, iksolver_to_tip_links));
00449   }
00450 
00451   return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
00452 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16