Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <ompl_ros_interface/state_transformers/ompl_ros_ik_state_transformer.h>
00038
00039 namespace ompl_ros_interface
00040 {
00041 OmplRosIKStateTransformer::OmplRosIKStateTransformer(const ompl::base::StateSpacePtr &state_space,
00042 const planning_models::KinematicModel::JointModelGroup* physical_joint_model_group) : OmplRosStateTransformer(state_space, physical_joint_model_group), kinematics_loader_("kinematics_base","kinematics::KinematicsBase")
00043
00044 {
00045 ros::NodeHandle node_handle("~");
00046 std::string kinematics_solver_name;
00047 group_name_ = state_space_->getName();
00048 physical_group_name_ = physical_joint_model_group_->getName();
00049 std::string base_name, tip_name;
00050
00051 if(!node_handle.hasParam(group_name_+"/kinematics_solver"))
00052 {
00053 ROS_ERROR("Kinematics solver not defined for group %s in namespace %s",group_name_.c_str(),node_handle.getNamespace().c_str());
00054 throw new OMPLROSException();
00055 }
00056 node_handle.getParam(group_name_+"/kinematics_solver",kinematics_solver_name);
00057
00058 if(!node_handle.hasParam(group_name_+"/root_name"))
00059 {
00060 ROS_ERROR_STREAM("Kinematics solver has no root name " << base_name << " in param ns " << node_handle.getNamespace());
00061 throw new OMPLROSException();
00062 }
00063 node_handle.getParam(group_name_+"/root_name",base_name);
00064
00065 if(!node_handle.hasParam(group_name_+"/tip_name"))
00066 {
00067 ROS_ERROR_STREAM("Kinematics solver has no root name " << tip_name << " in param ns " << node_handle.getNamespace());
00068 throw new OMPLROSException();
00069 }
00070 node_handle.getParam(group_name_+"/tip_name",tip_name);
00071
00072
00073 ROS_DEBUG("Trying to initialize solver %s",kinematics_solver_name.c_str());
00074 if(!kinematics_loader_.isClassAvailable(kinematics_solver_name))
00075 {
00076 ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str());
00077 throw new OMPLROSException();
00078 }
00079 ROS_DEBUG("Found solver %s",kinematics_solver_name.c_str());
00080
00081 try
00082 {
00083 kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
00084 }
00085 catch(pluginlib::PluginlibException& ex)
00086 {
00087 ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
00088 throw new OMPLROSException();
00089 }
00090 ROS_DEBUG("Loaded solver %s",kinematics_solver_name.c_str());
00091 if(!kinematics_solver_->initialize(group_name_,
00092 base_name,
00093 tip_name,
00094 .01))
00095 {
00096 ROS_ERROR("Could not initialize kinematics solver for group %s",group_name_.c_str());
00097 throw new OMPLROSException();
00098 }
00099 ROS_DEBUG("Initialized ompl ros state transformer %s",kinematics_solver_name.c_str());
00100 }
00101 }