bool checkFKService(kinematics_msgs::GetPositionFK::Request &request, kinematics_msgs::GetPositionFK::Response &response, const kinematics_msgs::KinematicSolverInfo &chain_info) { if(!checkLinkNames(request.fk_link_names,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.robot_state,chain_info)) { response.error_code.val = response.error_code.INVALID_ROBOT_STATE; 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; }