void planning_models::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
{
Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
Eigen::Quaterniond q;
quatFromMsg(transform.transform.rotation, q);
setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
} else {
ROS_ERROR("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
bool leatherman::poseFromMsg(const geometry_msgs::Pose &tmsg, Eigen::Affine3d &t)
{
Eigen::Quaterniond q; bool r = quatFromMsg(tmsg.orientation, q);
t = Eigen::Affine3d(Eigen::Translation3d(tmsg.position.x, tmsg.position.y, tmsg.position.z)*q.toRotationMatrix());
return r;
}