41 #include <boost/shared_ptr.hpp> 44 #include <planning_models/kinematic_state.h> 45 #include <rdf_loader/rdf_loader.h> 46 #include <urdf_interface/model.h> 47 #include <urdf/model.h> 48 #include <srdf/model.h> 53 const std::string& kinematics_solver_name,
const std::string& group_name,
54 const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
56 kinematics_loader_.reset(
new pluginlib::ClassLoader<kinematics::KinematicsBase>(
"kinematics_base",
"kinematics::" 63 catch (pluginlib::PluginlibException& ex)
65 ROS_ERROR(
"The plugin failed to load. Error: %s", ex.what());
69 if (!
kinematics_solver_->initialize(group_name, base_frame, tip_frame, search_discretization))
71 ROS_ERROR(
"Could not initialize solver");
75 rdf_loader::RDFLoader rdf_loader;
76 const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
77 const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
bool init(const kinematics_cache::KinematicsCache::Options &opt, const std::string &kinematics_solver_name, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
Initialization function.
planning_models::RobotModelPtr kinematic_model_
bool initialize(kinematics::KinematicsBaseConstPtr &solver, const planning_models::RobotModelConstPtr &kinematic_model, const KinematicsCache::Options &opt)
Initialize the cache class.
boost::shared_ptr< pluginlib::ClassLoader< kinematics::KinematicsBase > > kinematics_loader_
kinematics::KinematicsBase * kinematics_solver_