ArmKinematicsConstraintAware::ArmKinematicsConstraintAware(): kinematics_loader_("kinematics_base","kinematics::KinematicsBase"),node_handle_("~")
{
std::string group_name, kinematics_solver_name;
node_handle_.param<bool>("visualize_solution",visualize_solution_,true);
node_handle_.param<std::string>("group", group_, std::string());
node_handle_.param<std::string>("kinematics_solver",kinematics_solver_name," ");
ROS_INFO("Using kinematics solver name: %s",kinematics_solver_name.c_str());
if (group_.empty())
{
ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to monitor, node cannot compute collision aware inverse kinematics");
active_ = false;
return;
}
node_handle_.param<bool>("use_plugin_fk",use_plugin_fk_,false);
if (!use_plugin_fk_) ROS_INFO("Using TF for forward kinematics");
else ROS_INFO("Using solver for forward kinematics");
collision_models_interface_ = new planning_environment::CollisionModelsInterface("robot_description");
kinematics::KinematicsBase* kinematics_solver = NULL;
try
{
kinematics_solver = kinematics_loader_.createClassInstance(kinematics_solver_name);
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load. Error1: %s", ex.what()); //handle the class failing to load
return;
}
solver_ = new ArmKinematicsSolverConstraintAware(kinematics_solver,
collision_models_interface_,
group_);
active_ = solver_->isActive();
if(active_) {
getChainInfoFromRobotModel(*collision_models_interface_->getParsedDescription(),
solver_->getBaseName(),
solver_->getTipName(),
chain_info_);
}
advertiseBaseKinematicsServices();
advertiseConstraintIKService();
}
bool getChainInfo(const std::string &name,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
std::string urdf_xml, full_urdf_xml, root_name, tip_name;
ros::NodeHandle node_handle;
ros::NodeHandle private_handle("~"+name);
node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!node_handle.getParam(full_urdf_xml, result))
{
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root and Tip From Parameter Service
if (!private_handle.getParam("root_name", root_name))
{
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!private_handle.getParam("tip_name", tip_name))
{
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initString(result))
{
ROS_FATAL("Could not initialize robot model");
return -1;
}
if (!getChainInfoFromRobotModel(robot_model,root_name,tip_name,chain_info))
{
ROS_FATAL("Could not read information about the joints");
return false;
}
return true;
}