Why is tf2::doTransform not transforming my pointcloud?
The function tf2::doTransform does not seem to transform my pointcloud. I made a minimal example to showcase my problem or maybe I am misunderstanding something.
In rviz i subscribe to both topics: "/realsense_zr300/points2" and "/world/source", but the two pointclouds lie on each other, why? Shouldn't they be different because the source is in the realsense frame and the transformed cloud is in the world frame? I also checked the data field of the input and output and they are the same, how can this be if I am applying a transform? The transform sets the frame_id of the output correctly to world, but does not change any value in the data. Attached are two pictures to clarify how things look like:
EDIT: the frame id of the input cloud is “realsense_zr300_ir_optical_frame“ and of the output cloud it is “world“. The first few points are exactly the same in both clouds, I am not at the pc currently, but the valued were something similiar to [0,0,~179, ~240, 0,0,~ 179, ~240...] when I echoed the messages and looked at the data field. Thank you for the good edit.
I excpected this transformation to transform the pointcloud onto the floor, or atleast to not be the same as the input cloud. What am I doing wrong?
main.cpp
#include "my_class.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "test");
MyClass mc;
ros::spin();
return 0;
}
my_class.h
#ifndef CPP_MY_CLASS_H
#define CPP_MY_CLASS_H
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/PointCloud2.h>
class MyClass
{
public:
ros::NodeHandle nh_;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener tfListener_;
ros::Publisher W_source_pub_;
ros::Subscriber points_sub_;
MyClass();
void point_cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& msg);
};
#endif //CPP_MY_CLASS_H
my_class.cpp
#include "my_class.h"
MyClass::MyClass() : tfListener_(tfBuffer_)
{
points_sub_ = nh_.subscribe("/realsense_zr300/points2", 1, &MyClass::point_cloud_cb, this);
W_source_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/world/source", 10);
}
void MyClass::point_cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& msg) {
geometry_msgs::TransformStamped C_to_W_transform;
try {
C_to_W_transform = tfBuffer_.lookupTransform("world", "realsense_zr300_ir_optical_frame", ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(1.0).sleep();
return;
}
sensor_msgs::PointCloud2 w_cloud;
std::cout << C_to_W_transform.transform.translation.x << std::endl;
std::cout << C_to_W_transform.transform.translation.y << std::endl;
std::cout << C_to_W_transform.transform.translation.z << std::endl;
std::cout << C_to_W_transform.transform.rotation.x << std::endl;
std::cout << C_to_W_transform.transform.rotation.y << std::endl;
std::cout << C_to_W_transform.transform.rotation.z << std::endl;
std::cout << C_to_W_transform.transform.rotation.w << std::endl;
tf2::doTransform(*msg, w_cloud, C_to_W_transform);
W_source_pub_.publish(w_cloud);
}
I wrote a gazebo model plugin that publishes the tf tree. To check it is correct I print the values of the transform I get and they are, as they should be:
x: 0 y: -0.5 z: 0.5
x: -0.189251 y: 0.184585 z: 0.690409 w: 0.673385
The update loop of the plugin looks like this:
void ModelTf::OnUpdate() {
pose_ = this->model->GetWorldPose();
transformStamped_.header.stamp = ros::Time::now();
transformStamped_.header.frame_id = "world ...
I reformatted so users would see the question and images before the wall of code/xml.
Print out the first 3 or 4 xyz values of the transformed and untransformed point clouds, and the frame ids of each cloud, and update your question with those values.
It makes sense that the point clouds in rviz are on top of each other, but the underlying point xyz values should be different.
rostopic echo
is printing raw bytes not the floating point values, so although the bytes should be different printing the xyz from code is more informative.