kinematics_plugin_loader.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Dave Coleman */
36 
40 #include <boost/thread/mutex.hpp>
41 #include <sstream>
42 #include <vector>
43 #include <map>
44 #include <memory>
45 #include <ros/ros.h>
47 
49 {
50 static const std::string LOGNAME = "kinematics_plugin_loader";
51 
53 {
54 public:
62  KinematicsLoaderImpl(const std::string& robot_description,
63  const std::map<std::string, std::vector<std::string> >& possible_kinematics_solvers,
64  const std::map<std::string, std::vector<double> >& search_res,
65  const std::map<std::string, std::vector<std::string> >& iksolver_to_tip_links)
66  : robot_description_(robot_description)
67  , possible_kinematics_solvers_(possible_kinematics_solvers)
68  , search_res_(search_res)
69  , iksolver_to_tip_links_(iksolver_to_tip_links)
70  {
71  try
72  {
73  kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::"
74  "KinematicsBase"));
75  }
77  {
78  ROS_ERROR_NAMED(LOGNAME, "Unable to construct kinematics loader. Error: %s", e.what());
79  }
80  }
81 
87  std::vector<std::string> chooseTipFrames(const robot_model::JointModelGroup* jmg)
88  {
89  std::vector<std::string> tips;
90  std::map<std::string, std::vector<std::string> >::const_iterator ik_it =
91  iksolver_to_tip_links_.find(jmg->getName());
92 
93  // Check if tips were loaded onto the rosparam server previously
94  if (ik_it != iksolver_to_tip_links_.end())
95  {
96  // the tip is being chosen based on a corresponding rosparam ik link
97  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Choosing tip frame of kinematic solver for group "
98  << jmg->getName()
99  << " based on values in rosparam server.");
100  tips = ik_it->second;
101  }
102  else
103  {
104  // get the last link in the chain
105  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Choosing tip frame of kinematic solver for group "
106  << jmg->getName() << " based on last link in chain");
107 
108  tips.push_back(jmg->getLinkModels().back()->getName());
109  }
110 
111  // Error check
112  if (tips.empty())
113  {
114  ROS_ERROR_STREAM_NAMED("kinematics_plugin_loader", "Error choosing kinematic solver tip frame(s).");
115  }
116 
117  // Debug tip choices
118  std::stringstream tip_debug;
119  tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): ";
120  for (std::size_t i = 0; i < tips.size(); ++i)
121  tip_debug << tips[i] << ", ";
122  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", tip_debug.str());
123 
124  return tips;
125  }
126 
127  kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup* jmg)
128  {
129  kinematics::KinematicsBasePtr result;
130  if (!kinematics_loader_)
131  {
132  ROS_ERROR_NAMED(LOGNAME, "Invalid kinematics loader.");
133  return result;
134  }
135  if (!jmg)
136  {
137  ROS_ERROR_NAMED(LOGNAME, "Specified group is NULL. Cannot allocate kinematics solver.");
138  return result;
139  }
140  const std::vector<const robot_model::LinkModel*>& links = jmg->getLinkModels();
141  if (links.empty())
142  {
143  ROS_ERROR_NAMED(LOGNAME, "No links specified for group '%s'. Cannot allocate kinematics solver.",
144  jmg->getName().c_str());
145  return result;
146  }
147 
148  ROS_DEBUG_NAMED(LOGNAME, "Trying to allocate kinematics solver for group '%s'", jmg->getName().c_str());
149 
150  std::map<std::string, std::vector<std::string> >::const_iterator it =
151  possible_kinematics_solvers_.find(jmg->getName());
152  if (it == possible_kinematics_solvers_.end())
153  {
154  ROS_DEBUG_NAMED(LOGNAME, "No kinematics solver available for this group");
155  return result;
156  }
157 
158  const std::string& base = links.front()->getParentJointModel()->getParentLinkModel() ?
159  links.front()->getParentJointModel()->getParentLinkModel()->getName() :
160  jmg->getParentModel().getModelFrame();
161 
162  // just to be sure, do not call the same pluginlib instance allocation function in parallel
163  boost::mutex::scoped_lock slock(lock_);
164  for (std::size_t i = 0; !result && i < it->second.size(); ++i)
165  {
166  try
167  {
168  result = kinematics_loader_->createUniqueInstance(it->second[i]);
169  if (result)
170  {
171  // choose the tip of the IK solver
172  const std::vector<std::string> tips = chooseTipFrames(jmg);
173 
174  // choose search resolution
175  double search_res = search_res_.find(jmg->getName())->second[i]; // we know this exists, by construction
176 
177  if (!result->initialize(jmg->getParentModel(), jmg->getName(),
178  (base.empty() || base[0] != '/') ? base : base.substr(1), tips, search_res) &&
179  // on failure: fallback to old method (TODO: remove in future)
180  !result->initialize(robot_description_, jmg->getName(),
181  (base.empty() || base[0] != '/') ? base : base.substr(1), tips, search_res))
182  {
183  ROS_ERROR_NAMED(LOGNAME, "Kinematics solver of type '%s' could not be initialized for group '%s'",
184  it->second[i].c_str(), jmg->getName().c_str());
185  result.reset();
186  continue;
187  }
188 
189  result->setDefaultTimeout(jmg->getDefaultIKTimeout());
190  ROS_DEBUG_NAMED(LOGNAME,
191  "Successfully allocated and initialized a kinematics solver of type '%s' with search "
192  "resolution %lf for group '%s' at address %p",
193  it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
194  break;
195  }
196  }
198  {
199  ROS_ERROR_NAMED(LOGNAME, "The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
200  }
201  }
202 
203  if (!result)
204  {
205  ROS_DEBUG_NAMED(LOGNAME, "No usable kinematics solver was found for this group.\n"
206  "Did you load kinematics.yaml into your node's namespace?");
207  }
208  return result;
209  }
210 
211  // cache solver between two consecutive calls
212  // first call in RobotModelLoader::loadKinematicsSolvers() is just to check suitability for jmg
213  // second call in JointModelGroup::setSolverAllocators() is to actually retrieve the instance for use
214  kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup* jmg)
215  {
216  boost::mutex::scoped_lock slock(cache_lock_);
217  kinematics::KinematicsBasePtr& cached = instances_[jmg];
218  if (cached.unique())
219  return std::move(cached); // pass on unique instance
220 
221  // create a new instance and store in instances_
222  cached = allocKinematicsSolver(jmg);
223  return cached;
224  }
225 
226  void status() const
227  {
228  for (std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.begin();
229  it != possible_kinematics_solvers_.end(); ++it)
230  for (std::size_t i = 0; i < it->second.size(); ++i)
231  ROS_INFO_NAMED(LOGNAME, "Solver for group '%s': '%s' (search resolution = %lf)", it->first.c_str(),
232  it->second[i].c_str(), search_res_.at(it->first)[i]);
233  }
234 
235 private:
236  std::string robot_description_;
237  std::map<std::string, std::vector<std::string> > possible_kinematics_solvers_;
238  std::map<std::string, std::vector<double> > search_res_;
239  std::map<std::string, std::vector<std::string> > iksolver_to_tip_links_; // a map between each ik solver and a vector
240  // of custom-specified tip link(s)
241  std::shared_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase> > kinematics_loader_;
242  std::map<const robot_model::JointModelGroup*, kinematics::KinematicsBasePtr> instances_;
243  boost::mutex lock_;
244  boost::mutex cache_lock_;
245 };
246 
248 {
249  if (loader_)
250  loader_->status();
251  else
252  ROS_INFO_NAMED(LOGNAME, "Loader function was never required");
253 }
254 
255 robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction()
256 {
258  moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction");
259 
260  if (loader_)
261  return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
262 
264  robot_description_ = rml.getRobotDescription();
265  return getLoaderFunction(rml.getSRDF());
266 }
267 
268 robot_model::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const srdf::ModelSharedPtr& srdf_model)
269 {
271  moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)");
272 
273  if (!loader_)
274  {
275  ROS_DEBUG_NAMED(LOGNAME, "Configuring kinematics solvers");
276  groups_.clear();
277 
278  std::map<std::string, std::vector<std::string> > possible_kinematics_solvers;
279  std::map<std::string, std::vector<double> > search_res;
280  std::map<std::string, std::vector<std::string> > iksolver_to_tip_links;
281 
282  if (srdf_model)
283  {
284  const std::vector<srdf::Model::Group>& known_groups = srdf_model->getGroups();
285  if (default_search_resolution_ <= std::numeric_limits<double>::epsilon())
287 
288  if (default_solver_plugin_.empty())
289  {
290  ROS_DEBUG_NAMED(LOGNAME, "Loading settings for kinematics solvers from the ROS param server ...");
291 
292  // read data using ROS params
293  ros::NodeHandle nh("~");
294 
295  // read the list of plugin names for possible kinematics solvers
296  for (std::size_t i = 0; i < known_groups.size(); ++i)
297  {
298  std::string base_param_name = known_groups[i].name_;
299  ROS_DEBUG_NAMED("kinematics_plugin_loader", "Looking for param %s ",
300  (base_param_name + "/kinematics_solver").c_str());
301  std::string ksolver_param_name;
302  bool found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
303  if (!found || !nh.hasParam(ksolver_param_name))
304  {
305  base_param_name = robot_description_ + "_kinematics/" + known_groups[i].name_;
306  ROS_DEBUG_NAMED(LOGNAME, "Looking for param %s ", (base_param_name + "/kinematics_solver").c_str());
307  found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
308  }
309  if (found)
310  {
311  ROS_DEBUG_NAMED(LOGNAME, "Found param %s", ksolver_param_name.c_str());
312  std::string ksolver;
313  if (nh.getParam(ksolver_param_name, ksolver))
314  {
315  std::stringstream ss(ksolver);
316  bool first = true;
317  while (ss.good() && !ss.eof())
318  {
319  if (first)
320  {
321  first = false;
322  groups_.push_back(known_groups[i].name_);
323  }
324  std::string solver;
325  ss >> solver >> std::ws;
326  possible_kinematics_solvers[known_groups[i].name_].push_back(solver);
327  ROS_DEBUG_NAMED(LOGNAME, "Using kinematics solver '%s' for group '%s'.", solver.c_str(),
328  known_groups[i].name_.c_str());
329  }
330  }
331  }
332 
333  std::string ksolver_timeout_param_name;
334  if (nh.searchParam(base_param_name + "/kinematics_solver_timeout", ksolver_timeout_param_name))
335  {
336  double ksolver_timeout;
337  if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout))
338  ik_timeout_[known_groups[i].name_] = ksolver_timeout;
339  else
340  { // just in case this is an int
341  int ksolver_timeout_i;
342  if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout_i))
343  ik_timeout_[known_groups[i].name_] = ksolver_timeout_i;
344  }
345  }
346 
347  // TODO: Remove in future release (deprecated in PR #1288, Jan-2019, Melodic)
348  std::string ksolver_attempts_param_name;
349  if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name))
350  {
351  ROS_WARN_ONCE_NAMED(LOGNAME, "Kinematics solver doesn't support #attempts anymore, but only a timeout.\n"
352  "Please remove the parameter '%s' from your configuration.",
353  ksolver_attempts_param_name.c_str());
354  }
355 
356  std::string ksolver_res_param_name;
357  if (nh.searchParam(base_param_name + "/kinematics_solver_search_resolution", ksolver_res_param_name))
358  {
359  std::string ksolver_res;
360  if (nh.getParam(ksolver_res_param_name, ksolver_res))
361  {
362  std::stringstream ss(ksolver_res);
363  while (ss.good() && !ss.eof())
364  {
365  double res;
366  ss >> res >> std::ws;
367  search_res[known_groups[i].name_].push_back(res);
368  }
369  }
370  else
371  { // handle the case this param is just one value and parsed as a double
372  double res;
373  if (nh.getParam(ksolver_res_param_name, res))
374  search_res[known_groups[i].name_].push_back(res);
375  else
376  {
377  int res_i;
378  if (nh.getParam(ksolver_res_param_name, res_i))
379  search_res[known_groups[i].name_].push_back(res_i);
380  }
381  }
382  }
383 
384  // Allow a kinematic solver's tip link to be specified on the rosparam server
385  // Depreciated in favor of array version now
386  std::string ksolver_ik_link_param_name;
387  if (nh.searchParam(base_param_name + "/kinematics_solver_ik_link", ksolver_ik_link_param_name))
388  {
389  std::string ksolver_ik_link;
390  if (nh.getParam(ksolver_ik_link_param_name, ksolver_ik_link)) // has a custom rosparam-based tip link
391  {
392  ROS_WARN_STREAM_NAMED(LOGNAME, "Using kinematics_solver_ik_link rosparam is "
393  "deprecated in favor of kinematics_solver_ik_links "
394  "rosparam array.");
395  iksolver_to_tip_links[known_groups[i].name_].push_back(ksolver_ik_link);
396  }
397  }
398 
399  // Allow a kinematic solver's tip links to be specified on the rosparam server as an array
400  std::string ksolver_ik_links_param_name;
401  if (nh.searchParam(base_param_name + "/kinematics_solver_ik_links", ksolver_ik_links_param_name))
402  {
403  XmlRpc::XmlRpcValue ksolver_ik_links;
404  if (nh.getParam(ksolver_ik_links_param_name, ksolver_ik_links)) // has custom rosparam-based tip link(s)
405  {
406  if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray)
407  {
408  ROS_WARN_STREAM_NAMED(LOGNAME, "rosparam '" << ksolver_ik_links_param_name << "' should be an XmlRpc "
409  "value type 'Array'");
410  }
411  else
412  {
413  for (int32_t j = 0; j < ksolver_ik_links.size(); ++j)
414  {
415  ROS_ASSERT(ksolver_ik_links[j].getType() == XmlRpc::XmlRpcValue::TypeString);
416  ROS_DEBUG_STREAM_NAMED(LOGNAME, "found tip " << static_cast<std::string>(ksolver_ik_links[j])
417  << " for group " << known_groups[i].name_);
418  iksolver_to_tip_links[known_groups[i].name_].push_back(static_cast<std::string>(ksolver_ik_links[j]));
419  }
420  }
421  }
422  }
423 
424  // make sure there is a default resolution at least specified for every solver (in case it was not specified
425  // on the param server)
426  while (search_res[known_groups[i].name_].size() < possible_kinematics_solvers[known_groups[i].name_].size())
427  search_res[known_groups[i].name_].push_back(default_search_resolution_);
428  }
429  }
430  else
431  {
432  ROS_DEBUG_NAMED(LOGNAME, "Using specified default settings for kinematics solvers ...");
433  for (std::size_t i = 0; i < known_groups.size(); ++i)
434  {
435  possible_kinematics_solvers[known_groups[i].name_].resize(1, default_solver_plugin_);
436  search_res[known_groups[i].name_].resize(1, default_search_resolution_);
437  ik_timeout_[known_groups[i].name_] = default_solver_timeout_;
438  groups_.push_back(known_groups[i].name_);
439  }
440  }
441  }
442 
443  loader_.reset(
444  new KinematicsLoaderImpl(robot_description_, possible_kinematics_solvers, search_res, iksolver_to_tip_links));
445  }
446 
448 }
449 } // namespace kinematics_plugin_loader
std::shared_ptr< pluginlib::ClassLoader< kinematics::KinematicsBase > > kinematics_loader_
#define ROS_INFO_NAMED(name,...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
Default constructor.
Definition: rdf_loader.h:51
KinematicsLoaderImpl(const std::string &robot_description, const std::map< std::string, std::vector< std::string > > &possible_kinematics_solvers, const std::map< std::string, std::vector< double > > &search_res, const std::map< std::string, std::vector< std::string > > &iksolver_to_tip_links)
Pimpl Implementation of KinematicsLoader.
std::vector< std::string > chooseTipFrames(const robot_model::JointModelGroup *jmg)
Helper function to decide which, and how many, tip frames a planning group has.
#define ROS_DEBUG_NAMED(name,...)
Type const & getType() const
bool getParam(const std::string &key, std::string &s) const
std::map< std::string, std::vector< std::string > > possible_kinematics_solvers_
#define ROS_WARN_ONCE_NAMED(name,...)
bool hasParam(const std::string &key) const
std::map< std::string, std::vector< std::string > > iksolver_to_tip_links_
std::map< const robot_model::JointModelGroup *, kinematics::KinematicsBasePtr > instances_
robot_model::SolverAllocatorFn getLoaderFunction()
Get a function pointer that allocates and initializes a kinematics solver. If not previously called...
static const std::string LOGNAME
bool searchParam(const std::string &key, std::string &result) const
static const double DEFAULT_SEARCH_DISCRETIZATION
std::shared_ptr< Model > ModelSharedPtr
kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
#define ROS_ERROR_NAMED(name,...)
#define ROS_ASSERT(cond)
kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup *jmg)
#define ROS_WARN_STREAM_NAMED(name, args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Sep 20 2019 03:53:02