Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <ros/ros.h>
- // #include <sensor_msgs/PointCloud.h>
- // PCL specific includes
- #include <pcl/ros/conversions.h>
- #include <pcl/point_cloud.h>
- #include <pcl/point_types.h>
- #include <pcl/filters/voxel_grid.h>
- #include <pcl/visualization/pcl_visualizer.h>
- #include <pcl_ros/impl/transforms.hpp>
- #include <tf/transform_listener.h>
- #include <tf/transform_broadcaster.h>
- ros::Publisher pub;
- const double pi=3.14159265359;
- int main (int argc, char** argv)
- {
- // Initialize ROS
- ros::init (argc, argv, "my_pcl_tutorial");
- ros::NodeHandle nh;
- // Create a ROS publisher for the output point cloud
- pub = nh.advertise<sensor_msgs::PointCloud2> ("cicle_cloud", 1);
- // ------------------------------------
- // -----Create example point cloud-----
- // ------------------------------------
- pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
- std::cout << "Genarating example point clouds.\n\n";
- // the XYZRGB cloud will gradually go from red to green to blue.
- uint8_t r(255), g(15), b(15);
- for (float z(-1.0); z <= 1.0; z += 0.05)
- {
- for (float angle(0.0); angle <= 360.0; angle += 5.0)
- {
- pcl::PointXYZ basic_point;
- basic_point.x = 0.5 * cosf (angle*pi/180);
- basic_point.y = sinf (angle*pi/180);
- basic_point.z = z;
- basic_cloud_ptr->points.push_back(basic_point);
- pcl::PointXYZRGB point;
- point.x = basic_point.x;
- point.y = basic_point.y;
- point.z = basic_point.z;
- uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
- static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
- point.rgb = *reinterpret_cast<float*>(&rgb);
- point_cloud_ptr->points.push_back (point);
- }
- if (z < 0.0)
- {
- r -= 12;
- g += 12;
- }
- else
- {
- g -= 12;
- b += 12;
- }
- }
- ros::Rate loop_rate(10);
- sensor_msgs::PointCloud2 shape;
- pcl::toROSMsg(*point_cloud_ptr,shape);
- shape.header.frame_id="myshape";
- tf::TransformBroadcaster br;
- tf::Transform transform;
- transform.setOrigin( tf::Vector3(0.0, 0, 0.0) );
- transform.setRotation( tf::Quaternion(0, 0, 0) );
- while(nh.ok()){
- br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "myshape"));
- pub.publish (shape);
- loop_rate.sleep();
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement