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 {
51 {
52 public:
60  KinematicsLoaderImpl(const std::string& robot_description,
61  const std::map<std::string, std::vector<std::string> >& possible_kinematics_solvers,
62  const std::map<std::string, std::vector<double> >& search_res,
63  const std::map<std::string, std::vector<std::string> >& iksolver_to_tip_links)
64  : robot_description_(robot_description)
65  , possible_kinematics_solvers_(possible_kinematics_solvers)
66  , search_res_(search_res)
67  , iksolver_to_tip_links_(iksolver_to_tip_links)
68  {
69  try
70  {
71  kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::"
72  "KinematicsBase"));
73  }
75  {
76  ROS_ERROR("Unable to construct kinematics loader. Error: %s", e.what());
77  }
78  }
79 
85  std::vector<std::string> chooseTipFrames(const robot_model::JointModelGroup* jmg)
86  {
87  std::vector<std::string> tips;
88  std::map<std::string, std::vector<std::string> >::const_iterator ik_it =
89  iksolver_to_tip_links_.find(jmg->getName());
90 
91  // Check if tips were loaded onto the rosparam server previously
92  if (ik_it != iksolver_to_tip_links_.end())
93  {
94  // the tip is being chosen based on a corresponding rosparam ik link
95  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Chooing tip frame of kinematic solver for group "
96  << jmg->getName()
97  << " based on values in rosparam server.");
98  tips = ik_it->second;
99  }
100  else
101  {
102  // get the last link in the chain
103  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", "Chooing tip frame of kinematic solver for group "
104  << jmg->getName() << " based on last link in chain");
105 
106  tips.push_back(jmg->getLinkModels().back()->getName());
107  }
108 
109  // Error check
110  if (tips.empty())
111  {
112  ROS_ERROR_STREAM_NAMED("kinematics_plugin_loader", "Error choosing kinematic solver tip frame(s).");
113  }
114 
115  // Debug tip choices
116  std::stringstream tip_debug;
117  tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): ";
118  for (std::size_t i = 0; i < tips.size(); ++i)
119  tip_debug << tips[i] << ", ";
120  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", tip_debug.str());
121 
122  return tips;
123  }
124 
125  kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup* jmg)
126  {
127  kinematics::KinematicsBasePtr result;
128  if (!jmg)
129  {
130  ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
131  return result;
132  }
133 
134  ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());
135 
136  if (kinematics_loader_ && jmg)
137  {
138  std::map<std::string, std::vector<std::string> >::const_iterator it =
139  possible_kinematics_solvers_.find(jmg->getName());
140  if (it != possible_kinematics_solvers_.end())
141  {
142  // just to be sure, do not call the same pluginlib instance allocation function in parallel
143  boost::mutex::scoped_lock slock(lock_);
144 
145  for (std::size_t i = 0; !result && i < it->second.size(); ++i)
146  {
147  try
148  {
149  result = kinematics_loader_->createUniqueInstance(it->second[i]);
150  if (result)
151  {
152  const std::vector<const robot_model::LinkModel*>& links = jmg->getLinkModels();
153  if (!links.empty())
154  {
155  const std::string& base = links.front()->getParentJointModel()->getParentLinkModel() ?
156  links.front()->getParentJointModel()->getParentLinkModel()->getName() :
157  jmg->getParentModel().getModelFrame();
158 
159  // choose the tip of the IK solver
160  const std::vector<std::string> tips = chooseTipFrames(jmg);
161 
162  // choose search resolution
163  double search_res =
164  search_res_.find(jmg->getName())->second[i]; // we know this exists, by construction
165 
166  if (!result->initialize(robot_description_, jmg->getName(),
167  (base.empty() || base[0] != '/') ? base : base.substr(1), tips, search_res))
168  {
169  ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'",
170  it->second[i].c_str(), jmg->getName().c_str());
171  result.reset();
172  }
173  else
174  {
175  result->setDefaultTimeout(jmg->getDefaultIKTimeout());
176  ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search "
177  "resolution %lf for group '%s' at address %p",
178  it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
179  }
180  }
181  else
182  ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
183  }
184  }
186  {
187  ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
188  }
189  }
190  }
191  else
192  ROS_DEBUG("No kinematics solver available for this group");
193  }
194 
195  if (!result)
196  {
197  ROS_DEBUG("No usable kinematics solver was found for this group.");
198  ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
199  }
200  return result;
201  }
202 
203  kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup* jmg)
204  {
205  {
206  boost::mutex::scoped_lock slock(lock_);
207  const std::vector<kinematics::KinematicsBasePtr>& vi = instances_[jmg];
208  for (std::size_t i = 0; i < vi.size(); ++i)
209  if (vi[i].unique())
210  {
211  ROS_DEBUG("Reusing cached kinematics solver for group '%s'", jmg->getName().c_str());
212  return vi[i]; // this is safe since the shared_ptr is copied on stack BEFORE the destructors in scope get
213  // called
214  }
215  }
216 
217  kinematics::KinematicsBasePtr res = allocKinematicsSolver(jmg);
218 
219  {
220  boost::mutex::scoped_lock slock(lock_);
221  instances_[jmg].push_back(res);
222  return res;
223  }
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("Solver for group '%s': '%s' (search resolution = %lf)", it->first.c_str(), it->second[i].c_str(),
232  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*, std::vector<kinematics::KinematicsBasePtr> > instances_;
243  boost::mutex lock_;
244 };
245 }
246 
248 {
249  if (loader_)
250  loader_->status();
251  else
252  ROS_INFO("Loader function was never required");
253 }
254 
256 {
258  moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction");
259 
260  if (loader_)
262  _1);
263 
265  robot_description_ = rml.getRobotDescription();
266  return getLoaderFunction(rml.getSRDF());
267 }
268 
269 robot_model::SolverAllocatorFn
271 {
273  moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction(SRDF)");
274 
275  if (!loader_)
276  {
277  ROS_DEBUG("Configuring kinematics solvers");
278  groups_.clear();
279 
280  std::map<std::string, std::vector<std::string> > possible_kinematics_solvers;
281  std::map<std::string, std::vector<double> > search_res;
282  std::map<std::string, std::vector<std::string> > iksolver_to_tip_links;
283 
284  if (srdf_model)
285  {
286  const std::vector<srdf::Model::Group>& known_groups = srdf_model->getGroups();
287  if (default_search_resolution_ <= std::numeric_limits<double>::epsilon())
289 
290  if (default_solver_plugin_.empty())
291  {
292  ROS_DEBUG("Loading settings for kinematics solvers from the ROS param server ...");
293 
294  // read data using ROS params
295  ros::NodeHandle nh("~");
296 
297  // read the list of plugin names for possible kinematics solvers
298  for (std::size_t i = 0; i < known_groups.size(); ++i)
299  {
300  std::string base_param_name = known_groups[i].name_;
301  ROS_DEBUG_NAMED("kinematics_plugin_loader", "Looking for param %s ",
302  (base_param_name + "/kinematics_solver").c_str());
303  std::string ksolver_param_name;
304  bool found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
305  if (!found || !nh.hasParam(ksolver_param_name))
306  {
307  base_param_name = robot_description_ + "_kinematics/" + known_groups[i].name_;
308  ROS_DEBUG_NAMED("kinematics_plugin_loader", "Looking for param %s ",
309  (base_param_name + "/kinematics_solver").c_str());
310  found = nh.searchParam(base_param_name + "/kinematics_solver", ksolver_param_name);
311  }
312  if (found)
313  {
314  ROS_DEBUG_NAMED("kinematics_plugin_loader", "Found param %s", ksolver_param_name.c_str());
315  std::string ksolver;
316  if (nh.getParam(ksolver_param_name, ksolver))
317  {
318  std::stringstream ss(ksolver);
319  bool first = true;
320  while (ss.good() && !ss.eof())
321  {
322  if (first)
323  {
324  first = false;
325  groups_.push_back(known_groups[i].name_);
326  }
327  std::string solver;
328  ss >> solver >> std::ws;
329  possible_kinematics_solvers[known_groups[i].name_].push_back(solver);
330  ROS_DEBUG_NAMED("kinematics_plugin_loader", "Using kinematics solver '%s' for group '%s'.",
331  solver.c_str(), known_groups[i].name_.c_str());
332  }
333  }
334  }
335 
336  std::string ksolver_timeout_param_name;
337  if (nh.searchParam(base_param_name + "/kinematics_solver_timeout", ksolver_timeout_param_name))
338  {
339  double ksolver_timeout;
340  if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout))
341  ik_timeout_[known_groups[i].name_] = ksolver_timeout;
342  else
343  { // just in case this is an int
344  int ksolver_timeout_i;
345  if (nh.getParam(ksolver_timeout_param_name, ksolver_timeout_i))
346  ik_timeout_[known_groups[i].name_] = ksolver_timeout_i;
347  }
348  }
349 
350  std::string ksolver_attempts_param_name;
351  if (nh.searchParam(base_param_name + "/kinematics_solver_attempts", ksolver_attempts_param_name))
352  {
353  int ksolver_attempts;
354  if (nh.getParam(ksolver_attempts_param_name, ksolver_attempts))
355  ik_attempts_[known_groups[i].name_] = ksolver_attempts;
356  }
357 
358  std::string ksolver_res_param_name;
359  if (nh.searchParam(base_param_name + "/kinematics_solver_search_resolution", ksolver_res_param_name))
360  {
361  std::string ksolver_res;
362  if (nh.getParam(ksolver_res_param_name, ksolver_res))
363  {
364  std::stringstream ss(ksolver_res);
365  while (ss.good() && !ss.eof())
366  {
367  double res;
368  ss >> res >> std::ws;
369  search_res[known_groups[i].name_].push_back(res);
370  }
371  }
372  else
373  { // handle the case this param is just one value and parsed as a double
374  double res;
375  if (nh.getParam(ksolver_res_param_name, res))
376  search_res[known_groups[i].name_].push_back(res);
377  else
378  {
379  int res_i;
380  if (nh.getParam(ksolver_res_param_name, res_i))
381  search_res[known_groups[i].name_].push_back(res_i);
382  }
383  }
384  }
385 
386  // Allow a kinematic solver's tip link to be specified on the rosparam server
387  // Depreciated in favor of array version now
388  std::string ksolver_ik_link_param_name;
389  if (nh.searchParam(base_param_name + "/kinematics_solver_ik_link", ksolver_ik_link_param_name))
390  {
391  std::string ksolver_ik_link;
392  if (nh.getParam(ksolver_ik_link_param_name, ksolver_ik_link)) // has a custom rosparam-based tip link
393  {
394  ROS_WARN_STREAM_NAMED("kinematics_plugin_loader", "Using kinematics_solver_ik_link rosparam is "
395  "deprecated in favor of kinematics_solver_ik_links "
396  "rosparam array.");
397  iksolver_to_tip_links[known_groups[i].name_].push_back(ksolver_ik_link);
398  }
399  }
400 
401  // Allow a kinematic solver's tip links to be specified on the rosparam server as an array
402  std::string ksolver_ik_links_param_name;
403  if (nh.searchParam(base_param_name + "/kinematics_solver_ik_links", ksolver_ik_links_param_name))
404  {
405  XmlRpc::XmlRpcValue ksolver_ik_links;
406  if (nh.getParam(ksolver_ik_links_param_name, ksolver_ik_links)) // has custom rosparam-based tip link(s)
407  {
408  if (ksolver_ik_links.getType() != XmlRpc::XmlRpcValue::TypeArray)
409  {
410  ROS_WARN_STREAM_NAMED("kinematics_plugin_loader", "rosparam '"
411  << ksolver_ik_links_param_name
412  << "' should be an XmlRpc value type 'Array'");
413  }
414  else
415  {
416  for (int32_t j = 0; j < ksolver_ik_links.size(); ++j)
417  {
418  ROS_ASSERT(ksolver_ik_links[j].getType() == XmlRpc::XmlRpcValue::TypeString);
419  ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader",
420  "found tip " << static_cast<std::string>(ksolver_ik_links[j]) << " for group "
421  << known_groups[i].name_);
422  iksolver_to_tip_links[known_groups[i].name_].push_back(static_cast<std::string>(ksolver_ik_links[j]));
423  }
424  }
425  }
426  }
427 
428  // make sure there is a default resolution at least specified for every solver (in case it was not specified
429  // on the param server)
430  while (search_res[known_groups[i].name_].size() < possible_kinematics_solvers[known_groups[i].name_].size())
431  search_res[known_groups[i].name_].push_back(default_search_resolution_);
432  }
433  }
434  else
435  {
436  ROS_DEBUG("Using specified default settings for kinematics solvers ...");
437  for (std::size_t i = 0; i < known_groups.size(); ++i)
438  {
439  possible_kinematics_solvers[known_groups[i].name_].resize(1, default_solver_plugin_);
440  search_res[known_groups[i].name_].resize(1, default_search_resolution_);
441  ik_timeout_[known_groups[i].name_] = default_solver_timeout_;
442  ik_attempts_[known_groups[i].name_] = default_ik_attempts_;
443  groups_.push_back(known_groups[i].name_);
444  }
445  }
446  }
447 
448  loader_.reset(
449  new KinematicsLoaderImpl(robot_description_, possible_kinematics_solvers, search_res, iksolver_to_tip_links));
450  }
451 
453 }
std::shared_ptr< pluginlib::ClassLoader< kinematics::KinematicsBase > > kinematics_loader_
std::map< const robot_model::JointModelGroup *, std::vector< kinematics::KinematicsBasePtr > > instances_
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
Default constructor.
Definition: rdf_loader.h:53
int size() const
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::map< std::string, unsigned int > ik_attempts_
Type const & getType() const
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,...)
#define ROS_INFO(...)
bool searchParam(const std::string &key, std::string &result) const
std::map< std::string, std::vector< std::string > > possible_kinematics_solvers_
std::map< std::string, std::vector< std::string > > iksolver_to_tip_links_
robot_model::SolverAllocatorFn getLoaderFunction()
Get a function pointer that allocates and initializes a kinematics solver. If not previously called...
static const double DEFAULT_SEARCH_DISCRETIZATION
bool getParam(const std::string &key, std::string &s) const
kinematics::KinematicsBasePtr allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
bool hasParam(const std::string &key) const
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup *jmg)
#define ROS_WARN_STREAM_NAMED(name, args)
#define ROS_DEBUG(...)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32