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 if(!node_handle.hasParam(group_name_+"/kinematics_solver"))
00050 {
00051 ROS_ERROR("Kinematics solver not defined for group %s in namespace %s",group_name_.c_str(),node_handle.getNamespace().c_str());
00052 throw new OMPLROSException();
00053 }
00054 node_handle.getParam(group_name_+"/kinematics_solver",kinematics_solver_name);
00055 ROS_INFO("Trying to initialize solver %s",kinematics_solver_name.c_str());
00056 if(!kinematics_loader_.isClassAvailable(kinematics_solver_name))
00057 {
00058 ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str());
00059 throw new OMPLROSException();
00060 }
00061 ROS_INFO("Found solver %s",kinematics_solver_name.c_str());
00062
00063 try
00064 {
00065 kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
00066 }
00067 catch(pluginlib::PluginlibException& ex)
00068 {
00069 ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
00070 throw new OMPLROSException();
00071 }
00072 ROS_INFO("Loaded solver %s",kinematics_solver_name.c_str());
00073 if(!kinematics_solver_->initialize(physical_group_name_))
00074 {
00075 ROS_ERROR("Could not initialize kinematics solver for group %s",group_name_.c_str());
00076 throw new OMPLROSException();
00077 }
00078 ROS_INFO("Initialized ompl ros state transformer %s",kinematics_solver_name.c_str());
00079 }
00080 }