bool planning_environment::getLatestIdentityTransform(const std::string& to_frame,
const std::string& from_frame,
tf::TransformListener& tf,
tf::Transform& pose)
{
geometry_msgs::PoseStamped temp_pose;
temp_pose.pose.orientation.w = 1.0;
temp_pose.header.frame_id = from_frame;
geometry_msgs::PoseStamped trans_pose;
ros::Time tm;
std::string err_string;
if (tf.getLatestCommonTime(to_frame, temp_pose.header.frame_id, tm, &err_string) == tf::NO_ERROR) {
temp_pose.header.stamp = tm;
try {
tf.transformPose(to_frame, temp_pose, trans_pose);
} catch(tf::TransformException& ex) {
ROS_ERROR_STREAM("Unable to transform object from frame " << temp_pose.header.frame_id << " to " << to_frame << " error is " << ex.what());
return false;
}
} else {
ROS_ERROR_STREAM("No latest time for transforming " << from_frame << " to " << to_frame);
return false;
}
tf::poseMsgToTF(trans_pose.pose, pose);
return true;
}
void planning_environment::updateAttachedObjectBodyPoses(planning_environment::CollisionModels* cm,
planning_models::KinematicState& state,
tf::TransformListener& tf)
{
cm->bodiesLock();
const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();
if(link_att_objects.empty()) {
cm->bodiesUnlock();
return;
}
//this gets all the attached bodies in their correct current positions according to tf
geometry_msgs::PoseStamped ps;
ps.pose.orientation.w = 1.0;
for(std::map<std::string, std::map<std::string, bodies::BodyVector*> >::const_iterator it = link_att_objects.begin();
it != link_att_objects.end();
it++) {
ps.header.frame_id = it->first;
std::string es;
if (tf.getLatestCommonTime(cm->getWorldFrameId(), it->first, ps.header.stamp, &es) != tf::NO_ERROR) {
ROS_INFO_STREAM("Problem transforming into fixed frame from " << it->first << ". Error string " << es);
continue;
}
geometry_msgs::PoseStamped psout;
tf.transformPose(cm->getWorldFrameId(), ps, psout);
tf::Transform link_trans;
tf::poseMsgToTF(psout.pose, link_trans);
state.updateKinematicStateWithLinkAt(it->first, link_trans);
cm->updateAttachedBodyPosesForLink(state, it->first);
}
cm->bodiesUnlock();
}
bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
geometry_msgs::PoseStamped &pose_msg_out,
const std::string &root_frame,
tf::TransformListener& tf)
{
geometry_msgs::PoseStamped pose_msg_in = pose_msg;
ROS_DEBUG("Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
pose_msg_in.header.frame_id.c_str(),
pose_msg_in.pose.position.x,
pose_msg_in.pose.position.y,
pose_msg_in.pose.position.z,
pose_msg_in.pose.orientation.x,
pose_msg_in.pose.orientation.y,
pose_msg_in.pose.orientation.z,
pose_msg_in.pose.orientation.w);
pose_msg_out = pose_msg;
tf::Stamped<tf::Pose> pose_stamped;
poseStampedMsgToTF(pose_msg_in, pose_stamped);
if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
{
std::string err;
if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
{
ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'. TF said: %s",pose_stamped.frame_id_.c_str(),root_frame.c_str(), err.c_str());
return false;
}
}
try
{
tf.transformPose(root_frame, pose_stamped, pose_stamped);
}
catch(...)
{
ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
return false;
}
tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);
return true;
}