bool planning_environment::KinematicModelStateMonitor::setKinematicStateToTime(const ros::Time& time,
                                                                               planning_models::KinematicState& state) const
{
  std::map<std::string, double> joint_value_map;
  if(!getCachedJointStateValues(time, joint_value_map)) {
    return false;
  }
  state.setKinematicState(joint_value_map);
  return true;
}
コード例 #2
0
//returns true if the joint_state_map sets all the joints in the state, 
bool planning_environment::setRobotStateAndComputeTransforms(const arm_navigation_msgs::RobotState &robot_state,
                                                             planning_models::KinematicState& state)
{
  if(robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) {
    ROS_INFO_STREAM("Different number of names and positions: " << robot_state.joint_state.name.size() 
                    << " " << robot_state.joint_state.position.size());
    return false;
  }
  std::map<std::string, double> joint_state_map;
  for (unsigned int i = 0 ; i < robot_state.joint_state.name.size(); ++i)
  {
    joint_state_map[robot_state.joint_state.name[i]] = robot_state.joint_state.position[i];
  }
  std::vector<std::string> missing_states;
  bool complete = state.setKinematicState(joint_state_map, missing_states);
  if(!complete) {
    if(missing_states.empty()) {
      ROS_INFO("Incomplete, but no missing states");
    }
  }
  std::map<std::string, bool> has_missing_state_map;
  for(unsigned int i = 0; i < missing_states.size(); i++) {
    has_missing_state_map[missing_states[i]] = false;
  }
  bool need_to_update = false;
  if(robot_state.multi_dof_joint_state.joint_names.size() > 1) {
    ROS_INFO("More than 1 joint names");
  }
  for(unsigned int i = 0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++) {
    std::string joint_name = robot_state.multi_dof_joint_state.joint_names[i];
    if(!state.hasJointState(joint_name)) {
      ROS_WARN_STREAM("No joint matching multi-dof joint " << joint_name);
      continue;
    }
    planning_models::KinematicState::JointState* joint_state = state.getJointState(joint_name);
    if(robot_state.multi_dof_joint_state.frame_ids.size() <= i) {
      ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough frame ids");
    } else if(robot_state.multi_dof_joint_state.child_frame_ids.size() <= i) {
      ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough child frame ids");
    } else if(robot_state.multi_dof_joint_state.frame_ids[i] != joint_state->getParentFrameId() ||
              robot_state.multi_dof_joint_state.child_frame_ids[i] != joint_state->getChildFrameId()) {
      ROS_WARN_STREAM("Robot state msg has bad multi_dof transform - frame ids or child frame_ids do not match up with joint");
    } else {
      tf::StampedTransform transf;
      tf::poseMsgToTF(robot_state.multi_dof_joint_state.poses[i], transf);
      joint_state->setJointStateValues(transf);
      need_to_update = true;
      //setting true for all individual joint names because we're setting the transform
      for(unsigned int i = 0; i < joint_state->getJointStateNameOrder().size(); i++) {
        has_missing_state_map[joint_state->getJointStateNameOrder()[i]] = true;
      }
    }
  }
  if(need_to_update) {
    state.updateKinematicLinks();
  }
  if(!complete) {
    for(std::map<std::string, bool>::iterator it = has_missing_state_map.begin();
        it != has_missing_state_map.end();
        it++) {
      if(!it->second) {
        return false;
      }
    }
    return true;
  }
  return true;
}
void planning_environment::KinematicModelStateMonitor::setStateValuesFromCurrentValues(planning_models::KinematicState& state) const
{
  current_joint_values_lock_.lock();
  state.setKinematicState(current_joint_state_map_);
  current_joint_values_lock_.unlock();
}