Rviz pointcloud2 Position Transformer
Hi guys. i am trying to convert pointcloud2 data with different tf.
I want to convert data "map"frame to "new_map" frame i convert data but rviz position transformer does not have XYZ. Below is my code.
static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
static pcl::PointCloud<pcl::PointXYZ> map;
static tf::StampedTransform local_transform;
pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
ROS_INFO("map_callback ");
if (points_map_num != input->width)
{
std::cout << "Update points_map." << std::endl;
points_map_num = input->width;
std::cout << "points_map_num : "<<points_map_num << std::endl;
std::cout << "frane_id : "<< input->header.frame_id <<std::endl;
std::cout << "height : "<< input->height <<std::endl;
// Convert the data type(from sensor_msgs to pcl).
pcl::fromROSMsg(*input, map);
tf::TransformListener local_transform_listener;
try
{
ros::Time now = ros::Time(0);
local_transform_listener.waitForTransform("/map", "/new_map", now, ros::Duration(20.0));
//local_transform_listener.lookupTransform("/map", "world", now, local_transform);
}
catch (tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
}
//pcl_ros::transformPointCloud(map, map, local_transform.inverse());
pcl_ros::transformPointCloud("new_map", map, transformed_cloud, local_transform_listener);
pcl::toROSMsg(transformed_cloud, transformed_cloud_msg);
//transformed_cloud_msg.header.frame_id = "new_map_tf";
transformed_cloud_msg.fields = input->fields;
std::cout << "transforemd width = "<< transformed_cloud_msg.width << std::endl;
std::cout << "transformed frame_id = " << transformed_cloud_msg.header.frame_id << std::endl;
std::cout << "height : "<< transformed_cloud_msg.height <<std::endl;
transformed_cloud_pub.publish(transformed_cloud_msg);
ROS_INFO("transformed!");
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "transform_map");
ros::NodeHandle node;
ros::NodeHandle nh;
transformed_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/transformed_point",1);
ros::Subscriber map_sub = nh.subscribe("points_map",1,map_callback);
ros::spin();
return 0;
}
input data's width is same to output'data. but rviz doesn't show the position transformer and colortransformer
plz help me