Rviz pointcloud2 Position Transformer

asked 2019-05-04 07:39:54 -0500

jhw3169@gmail.com gravatar image

updated 2019-05-04 08:42:20 -0500

gvdhoorn gravatar image

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

edit retag flag offensive close merge delete