Get RPY from tfListener
Hi, I want to get the RPY data from my transformation. I have the next code:
include <ros/ros.h>
include <tf/tf.h>
include <tf/transform_listener.h>
include <geometry_msgs/TransformStamped.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "tf_listener");
ros::NodeHandle nh;
ROS_INFO_STREAM("Started node tf_listener.");
std::string target_frame, source_frame;
tf::TransformListener tf_listener;
tf::StampedTransform transform;
ros::Rate rate(1.);
if (argc < 3) {
return -1;
}
else {
target_frame = argv[1];
source_frame = argv[2];
}
while (ros::ok()) {
try {
tf_listener.lookupTransform(target_frame, source_frame, ros::Time(), transform);
geometry_msgs::Transform buffer;
tf::transformTFToMsg(transform, buffer);
ROS_INFO_STREAM("Transform from " << target_frame << " to " << source_frame << ": " << std::endl << buffer);
ROS_INFO_STREAM("Distance between transforms: " << transform.getOrigin().length() << " meters.");
}
catch (...) {
ROS_WARN_STREAM("Waiting for transform from " << target_frame << " to " << source_frame);
}
rate.sleep();
}
return 0;
}
How can I get it??
Thanks.