bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
kinematics_msgs::GetPositionFK::Response &response)
{
if(!active_)
{
ROS_ERROR("FK service not active");
return true;
}
if(!checkFKService(request,response,fk_solver_info_))
return true;
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
jnt_pos_in.resize(dimension_);
for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++)
{
int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
if(tmp_index >=0)
jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
}
response.pose_stamped.resize(request.fk_link_names.size());
response.fk_link_names.resize(request.fk_link_names.size());
bool valid = true;
for(unsigned int i=0; i < request.fk_link_names.size(); i++)
{
ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
{
tf_pose.frame_id_ = root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(p_out,tf_pose);
tf::poseStampedTFToMsg(tf_pose,pose);
if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
}
else
{
ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
}
return true;
}
bool ArmKinematicsConstraintAware::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
kinematics_msgs::GetPositionFK::Response &response)
{
if(!active_)
{
ROS_ERROR("FK service not active");
return true;
}
if(!checkFKService(request,response,chain_info_))
return true;
planning_models::KinematicState* state;
if(collision_models_interface_->isPlanningSceneSet()) {
state = new planning_models::KinematicState(*collision_models_interface_->getPlanningSceneState());
} else {
state = new planning_models::KinematicState(collision_models_interface_->getKinematicModel());
state->setKinematicStateToDefault();
}
planning_environment::setRobotStateAndComputeTransforms(request.robot_state,
*state);
std::vector<geometry_msgs::Pose> poses;
bool valid;
if (!use_plugin_fk_) valid = solver_->getPositionFK(state,request.fk_link_names,poses);
else valid = solver_->getPluginsPositionFK(request.fk_link_names,request.robot_state.joint_state.position, poses);
if(valid) {
response.pose_stamped.resize( poses.size() );
response.fk_link_names.resize( poses.size() );
for(unsigned int i=0; i < poses.size(); i++) {
std_msgs::Header world_header;
world_header.frame_id = collision_models_interface_->getWorldFrameId();
if(!collision_models_interface_->convertPoseGivenWorldTransform(*state,
request.header.frame_id,
world_header,
poses[i],
response.pose_stamped[i])) {
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return true;
}
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
}
}
else
{
ROS_ERROR("Could not compute FK");
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
delete state;
return valid;
}
