40 #include <boost/thread/mutex.hpp> 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)
76 ROS_ERROR(
"Unable to construct kinematics loader. Error: %s", e.what());
87 std::vector<std::string> tips;
88 std::map<std::string, std::vector<std::string> >::const_iterator ik_it =
97 <<
" based on values in rosparam server.");
104 << jmg->getName() <<
" based on last link in chain");
106 tips.push_back(jmg->getLinkModels().back()->getName());
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] <<
", ";
127 kinematics::KinematicsBasePtr result;
130 ROS_ERROR(
"Specified group is NULL. Cannot allocate kinematics solver.");
134 ROS_DEBUG(
"Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());
138 std::map<std::string, std::vector<std::string> >::const_iterator it =
143 boost::mutex::scoped_lock slock(
lock_);
145 for (std::size_t i = 0; !result && i < it->second.size(); ++i)
152 const std::vector<const robot_model::LinkModel*>& links = jmg->getLinkModels();
155 const std::string& base = links.front()->getParentJointModel()->getParentLinkModel() ?
156 links.front()->getParentJointModel()->getParentLinkModel()->getName() :
157 jmg->getParentModel().getModelFrame();
167 (base.empty() || base[0] !=
'/') ? base : base.substr(1), tips, search_res))
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());
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());
182 ROS_ERROR(
"No links specified for group '%s'", jmg->getName().c_str());
187 ROS_ERROR(
"The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
192 ROS_DEBUG(
"No kinematics solver available for this group");
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?");
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)
211 ROS_DEBUG(
"Reusing cached kinematics solver for group '%s'", jmg->getName().c_str());
220 boost::mutex::scoped_lock slock(
lock_);
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(),
242 std::map<const robot_model::JointModelGroup*, std::vector<kinematics::KinematicsBasePtr> >
instances_;
252 ROS_INFO(
"Loader function was never required");
269 robot_model::SolverAllocatorFn
277 ROS_DEBUG(
"Configuring kinematics solvers");
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;
286 const std::vector<srdf::Model::Group>& known_groups = srdf_model->getGroups();
292 ROS_DEBUG(
"Loading settings for kinematics solvers from the ROS param server ...");
298 for (std::size_t i = 0; i < known_groups.size(); ++i)
300 std::string base_param_name = known_groups[i].name_;
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))
309 (base_param_name +
"/kinematics_solver").c_str());
310 found = nh.
searchParam(base_param_name +
"/kinematics_solver", ksolver_param_name);
314 ROS_DEBUG_NAMED(
"kinematics_plugin_loader",
"Found param %s", ksolver_param_name.c_str());
316 if (nh.
getParam(ksolver_param_name, ksolver))
318 std::stringstream ss(ksolver);
320 while (ss.good() && !ss.eof())
325 groups_.push_back(known_groups[i].name_);
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());
336 std::string ksolver_timeout_param_name;
337 if (nh.
searchParam(base_param_name +
"/kinematics_solver_timeout", ksolver_timeout_param_name))
339 double ksolver_timeout;
340 if (nh.
getParam(ksolver_timeout_param_name, ksolver_timeout))
341 ik_timeout_[known_groups[i].name_] = ksolver_timeout;
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;
350 std::string ksolver_attempts_param_name;
351 if (nh.
searchParam(base_param_name +
"/kinematics_solver_attempts", ksolver_attempts_param_name))
353 int ksolver_attempts;
354 if (nh.
getParam(ksolver_attempts_param_name, ksolver_attempts))
358 std::string ksolver_res_param_name;
359 if (nh.
searchParam(base_param_name +
"/kinematics_solver_search_resolution", ksolver_res_param_name))
361 std::string ksolver_res;
362 if (nh.
getParam(ksolver_res_param_name, ksolver_res))
364 std::stringstream ss(ksolver_res);
365 while (ss.good() && !ss.eof())
368 ss >> res >> std::ws;
369 search_res[known_groups[i].name_].push_back(res);
375 if (nh.
getParam(ksolver_res_param_name, res))
376 search_res[known_groups[i].name_].push_back(res);
380 if (nh.
getParam(ksolver_res_param_name, res_i))
381 search_res[known_groups[i].name_].push_back(res_i);
388 std::string ksolver_ik_link_param_name;
389 if (nh.
searchParam(base_param_name +
"/kinematics_solver_ik_link", ksolver_ik_link_param_name))
391 std::string ksolver_ik_link;
392 if (nh.
getParam(ksolver_ik_link_param_name, ksolver_ik_link))
395 "deprecated in favor of kinematics_solver_ik_links " 397 iksolver_to_tip_links[known_groups[i].name_].push_back(ksolver_ik_link);
402 std::string ksolver_ik_links_param_name;
403 if (nh.
searchParam(base_param_name +
"/kinematics_solver_ik_links", ksolver_ik_links_param_name))
406 if (nh.
getParam(ksolver_ik_links_param_name, ksolver_ik_links))
411 << ksolver_ik_links_param_name
412 <<
"' should be an XmlRpc value type 'Array'");
416 for (int32_t j = 0; j < ksolver_ik_links.
size(); ++j)
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]));
430 while (search_res[known_groups[i].name_].size() < possible_kinematics_solvers[known_groups[i].name_].size())
436 ROS_DEBUG(
"Using specified default settings for kinematics solvers ...");
437 for (std::size_t i = 0; i < known_groups.size(); ++i)
443 groups_.push_back(known_groups[i].name_);
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)
std::string default_solver_plugin_
std::map< std::string, double > ik_timeout_
double default_solver_timeout_
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, std::vector< double > > search_res_
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.
double default_search_resolution_
#define ROS_DEBUG_NAMED(name,...)
bool searchParam(const std::string &key, std::string &result) const
std::string robot_description_
std::map< std::string, std::vector< std::string > > possible_kinematics_solvers_
KinematicsLoaderImplPtr loader_
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
unsigned int default_ik_attempts_
std::vector< std::string > groups_
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
kinematics::KinematicsBasePtr allocKinematicsSolverWithCache(const robot_model::JointModelGroup *jmg)
#define ROS_WARN_STREAM_NAMED(name, args)