Exemplo n.º 1
0
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;
}