how to use tf2 to transform PoseStamped messages from one frame to another?
Hi I am new to ROS so I apologize if my questions seem all over the place. I am simulating an autonomous quadcopter in gazebo, using aruco marker detection for landing. As pictured below, I have a map frame which is connected to the camera frame (base_link) which is then linked to the marker_frame. My goal is to transform the PoseStamped messages from the marker frame to the frame of the quadcopter model. And so, I have a few general inquries.
TF_Tree
- [fcu] <---> [fcu_frd] (connected to each other)
- [map]--->[base_link]--->[marker_frame]
- [local_origin]--->[local_origin_ned]
(For some reason there are 3 separate trees)
Questions
- I am confused as to how I am to transform the PoseStamped message from the Marker frame into the quadcopter frame. Correct me if I am wrong but is the local_origin_ned the frame for the quadcopter?
- I have created a broadcaster node that transforms the posestamped message from the marker frame to the camera frame however, I am not sure I am transforming the information right. Is this even the right way to start? I am quite confused. (pasted below)
I am using ros kinetic on ubuntu 16.04. Again, sorry for the unorganized thoughts, and thanks in advance.
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
void callback(const geometry_msgs::PoseStamped& msg){
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "base_link" ;
transformStamped.child_frame_id = "marker_frame";
transformStamped.transform.translation.x = msg.pose.position.x;
transformStamped.transform.translation.y = msg.pose.position.y;
transformStamped.transform.translation.z = msg.pose.position.z;
transformStamped.transform.rotation.x = msg.pose.orientation.x;
transformStamped.transform.rotation.y = msg.pose.orientation.y;
transformStamped.transform.rotation.z = msg.pose.orientation.z;
transformStamped.transform.rotation.w = msg.pose.orientation.w;
br.sendTransform(transformStamped);
}
int main(int argc, char** argv){
ros::init(argc,argv,"broadcast_cm");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("aruco_single/pose", 100, &callback);
ros::spin();
return 0;
}
Listener Node
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::PoseStamped poseIn;
geometry_msgs::PoseStamped poseOut;
bool received;
ros::Duration timeout;
void callback(const geometry_msgs::PoseStamped& msg){
poseIn = msg;
received= true;
}
int main(int argc, char** argv){
ros::init(argc, argv, "listener_test");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("aruco_single/pose", 100, &callback);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0); //rate 10
sleep(1);
while (nh.ok()){
try{
if(received){poseOut = tfBuffer.transform(poseIn, "map", ros::Duration(0.0));}
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
Just making sure: have you seen the tf2/Tutorials?
yes, I have.