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; }
//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(); }