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