void stampedTFDifferenceStampedTF(tf::StampedTransform &tf1,
tf::StampedTransform &tf2,
tf::StampedTransform &tfResult)
{
tfResult.setData(tf1.inverseTimes(tf2));
}
void poseStampedToStampedTF(geometry_msgs::PoseStamped &msg, tf::StampedTransform &stampedTF, std::string child)
{
tf::Transform btTrans;
stampedTF.stamp_ = msg.header.stamp;
stampedTF.frame_id_ = msg.header.frame_id;
stampedTF.child_frame_id_ = child;
tf::poseMsgToTF(msg.pose, btTrans);
stampedTF.setData(btTrans);
}
bool OdomTf::multiply_stamped_tfs(tf::StampedTransform A_stf,
tf::StampedTransform B_stf, tf::StampedTransform &C_stf) {
//long-winded approach:
//std::string str1(A_stf.child_frame_id_); //want to compare strings to check consistency
//std::string str2(B_stf.frame_id_);
//if (str1.compare(str2) != 0) { //SHOULD get that child frame of A is parent frame of B
//more compact approach:
if (A_stf.child_frame_id_.compare(B_stf.frame_id_) != 0) {
std::cout << "can't multiply transforms; mismatched frames" << endl;
std::cout << A_stf.child_frame_id_ << " is not " << B_stf.frame_id_ << '\n';
return false;
}
//if here, the named frames are logically consistent
tf::Transform C = A_stf*B_stf; //multiplication is defined for transforms
C_stf.setData(C);
C_stf.frame_id_ = A_stf.frame_id_;
C_stf.child_frame_id_ = B_stf.child_frame_id_;
C_stf.stamp_ = ros::Time::now();
//long-winded approach, equivalent to above:
/*
tf::Transform A, B; //simple transforms--not stamped
A = get_tf_from_stamped_tf(A_stf); // get the transform from the stamped transform
B = get_tf_from_stamped_tf(B_stf);
C = A*B; //multiplication is defined for transforms
C_stf.frame_id_ = A_stf.frame_id_; //assign appropriate parent and child frames to result
C_stf.child_frame_id_ = B_stf.child_frame_id_;
C_stf.setOrigin(C.getOrigin()); //populate the origin and orientation of the result
C_stf.setBasis(C.getBasis());
C_stf.stamp_ = ros::Time::now(); //assign the time stamp to current time;
* */
// alternatively, could assign this to the OLDER of A or B transforms
return true; //if got here, the multiplication is valid
}