Advertisement
Guest User

Creating PCL with a TF in ROS

a guest
Dec 10th, 2013
543
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.36 KB | None | 0 0
  1. #include <ros/ros.h>
  2. // #include <sensor_msgs/PointCloud.h>
  3. // PCL specific includes
  4. #include <pcl/ros/conversions.h>
  5. #include <pcl/point_cloud.h>
  6. #include <pcl/point_types.h>
  7. #include <pcl/filters/voxel_grid.h>
  8. #include <pcl/visualization/pcl_visualizer.h>
  9. #include <pcl_ros/impl/transforms.hpp>
  10.  
  11. #include <tf/transform_listener.h>
  12. #include <tf/transform_broadcaster.h>
  13.  
  14. ros::Publisher pub;
  15.  
  16. const double pi=3.14159265359;
  17.  
  18. int main (int argc, char** argv)
  19. {
  20.     // Initialize ROS
  21.     ros::init (argc, argv, "my_pcl_tutorial");
  22.     ros::NodeHandle nh;
  23.  
  24.     // Create a ROS publisher for the output point cloud
  25.     pub = nh.advertise<sensor_msgs::PointCloud2> ("cicle_cloud", 1);
  26.  
  27.     // ------------------------------------
  28.     // -----Create example point cloud-----
  29.     // ------------------------------------
  30.     pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  31.     pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
  32.     std::cout << "Genarating example point clouds.\n\n";
  33.     // the XYZRGB cloud will gradually go from red to green to blue.
  34.     uint8_t r(255), g(15), b(15);
  35.     for (float z(-1.0); z <= 1.0; z += 0.05)
  36.         {
  37.             for (float angle(0.0); angle <= 360.0; angle += 5.0)
  38.                 {
  39.                     pcl::PointXYZ basic_point;
  40.                     basic_point.x = 0.5 * cosf (angle*pi/180);
  41.                     basic_point.y = sinf (angle*pi/180);
  42.                     basic_point.z = z;
  43.                     basic_cloud_ptr->points.push_back(basic_point);
  44.  
  45.                     pcl::PointXYZRGB point;
  46.                     point.x = basic_point.x;
  47.                     point.y = basic_point.y;
  48.                     point.z = basic_point.z;
  49.                     uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
  50.                                     static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
  51.                     point.rgb = *reinterpret_cast<float*>(&rgb);
  52.                     point_cloud_ptr->points.push_back (point);
  53.                 }
  54.             if (z < 0.0)
  55.                 {
  56.                     r -= 12;
  57.                     g += 12;
  58.                 }
  59.             else
  60.                 {
  61.                     g -= 12;
  62.                     b += 12;
  63.                 }
  64.         }
  65.  
  66.     ros::Rate loop_rate(10);
  67.     sensor_msgs::PointCloud2 shape;
  68.     pcl::toROSMsg(*point_cloud_ptr,shape);
  69.     shape.header.frame_id="myshape";
  70.     tf::TransformBroadcaster br;
  71.     tf::Transform transform;
  72.     transform.setOrigin( tf::Vector3(0.0, 0, 0.0) );
  73.     transform.setRotation( tf::Quaternion(0, 0, 0) );
  74.  
  75.     while(nh.ok()){
  76.         br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "myshape"));
  77.         pub.publish (shape);
  78.         loop_rate.sleep();
  79.     }
  80. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement