bool checkLinkNames(const std::vector<std::string> &link_names,
const kinematics_msgs::KinematicSolverInfo &chain_info)
{
if(link_names.empty())
return false;
for(unsigned int i=0; i < link_names.size(); i++)
{
if(!checkLinkName(link_names[i],chain_info))
return false;
}
return true;
}
bool checkCollisionFreeIKService(kinematics_msgs::GetCollisionFreePositionIK::Request &request,
kinematics_msgs::GetCollisionFreePositionIK::Response &response,
const kinematics_msgs::KinematicTreeInfo &chain_info)
{
if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
{
ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for3.");
response.error_code.val = response.error_code.INVALID_LINK_NAME;
return false;
}
if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
{
response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
return false;
}
if(request.timeout <= ros::Duration(0.0))
request.timeout = ros::Duration(IK_DEFAULT_TIMEOUT);
return true;
}
bool checkIKService(moveit_msgs::GetPositionIK::Request &request,
moveit_msgs::GetPositionIK::Response &response,
const moveit_msgs::KinematicSolverInfo &chain_info)
{
if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
{
ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
response.error_code.val = response.error_code.INVALID_LINK_NAME;
return false;
}
if(!checkRobotState(request.ik_request.robot_state,chain_info))
{
response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
return false;
}
if(request.ik_request.timeout <= ros::Duration(0.0))
{
response.error_code.val = response.error_code.TIMED_OUT;
return false;
}
return true;
}
