kinematics_cache_ros.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2012, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Sachin Chitta
36 *********************************************************************/
37 
39 
40 // ROS
41 #include <boost/shared_ptr.hpp>
42 
43 // MoveIt!
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>
49 
50 namespace kinematics_cache_ros
51 {
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)
55 {
56  kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("kinematics_base", "kinematics::"
57  "KinematicsBase"));
58 
59  try
60  {
61  kinematics_solver_ = kinematics_loader_->createClassInstance(kinematics_solver_name);
62  }
63  catch (pluginlib::PluginlibException& ex) // handle the class failing to load
64  {
65  ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
66  return false;
67  }
68 
69  if (!kinematics_solver_->initialize(group_name, base_frame, tip_frame, search_discretization))
70  {
71  ROS_ERROR("Could not initialize solver");
72  return false;
73  }
74 
75  rdf_loader::RDFLoader rdf_loader;
76  const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
77  const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
78  kinematic_model_.reset(new planning_models::RobotModel(urdf_model, srdf));
79 
80  if (!initialize((kinematics::KinematicsBaseConstPtr&)kinematics_solver_,
81  (planning_models::RobotModelConstPtr&)kinematic_model_, opt))
82  {
83  return false;
84  }
85 
86  return true;
87 }
88 }
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_


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Wed Jul 10 2019 04:03:02