Trying to apply a rotation of one frame to another. Multiple callback transform?
I have two frames set up: prism_frame
and imu_frame
. The IMU frame is translated in XYZ for (0, 0, 0.3) and get's the rotations via a callback function using the standard tutorial approach:
node.subscribe("/imu_data", 10, &imuCallback)
where the callback function set's up the frame with prism_frame
as the parent frame and imu_frame
as the child
The prism_frame
gets tracked and has also a subscription to local xyz data using a callback functions which also defines a frame. My Problem is that the prism_frame
is in a rigid relation to the imu_frame
and therefore should also get the rotations.
I tried to solve it with two different callback function on the same frame setup with prism_frame
as parent and imu_frame
as child, but it didn't worked out since im overwriting them constantly... i really don't know how to solve this issue although it actually should be a simple one.. I was wondering if there is a possibility to process more than one message callbacks to define a frame?
Currently i'm trying to set um a lookupTransform in order to perform a tf2::doTransform on the frames, but i have the feeling that i don't really got the idea of how to use this correctly...
here is the code:
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
void ts2prismCallback(const geometry_msgs::PointStampedConstPtr& msg){
static tf2_ros::TransformBroadcaster tf2_broadcaster;
geometry_msgs::TransformStamped transformStamped;
// ROS_INFO("msg x: %f", msg->point.x);
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "ts_frame";
transformStamped.child_frame_id = "prism_frame";
// transform from map to tachy_frame with (1,1,1)
transformStamped.transform.translation.x = msg->point.x;
transformStamped.transform.translation.y = msg->point.y;
transformStamped.transform.translation.z = msg->point.z;
tf2::Quaternion q;
q.setRPY(0,0,0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
tf2_broadcaster.sendTransform(transformStamped);
}
void prismRotationCallback(const sensor_msgs::ImuConstPtr& msg){
static tf2_ros::TransformBroadcaster tf2_broadcaster;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "prism_frame";
transformStamped.child_frame_id = "imu_frame";
transformStamped.transform.translation.x = 0;
transformStamped.transform.translation.y = 0;
transformStamped.transform.translation.z = 0.3;
tf2::Quaternion q;
transformStamped.transform.rotation.x = msg->orientation.x;
transformStamped.transform.rotation.y = msg->orientation.y;
transformStamped.transform.rotation.z = msg->orientation.z;
transformStamped.transform.rotation.w = msg->orientation.w;
tf2_broadcaster.sendTransform(transformStamped);
}
int main(int argc, char** argv){
ros::init(argc, argv, "tf_ts2prism");
ros::NodeHandle node;
ros::Subscriber sub_ts = node.subscribe("/ts_points", 10, &ts2prismCallback);
ros::Subscriber sub_imu = node.subscribe("/imu/data_raw", 10, &prismRotationCallback);
ros::spin();
return 0;
}
and now the listener code
#include <ros/ros.h>
#include "std_msgs/String.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PointStamped.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h ...