Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
85 lines (64 sloc) 2.42 KB
//
// Created by neil on 4/1/19.
//
#include <ros/ros.h>
#include <ros/console.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_ros/transforms.h>
ros::Publisher pub;
tf::Transform transform;
tf::Quaternion toQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
// Abbreviations for the various angular functions
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
tf::Quaternion q = tf::Quaternion(0, 0, 0, 0);
q.setW(cy * cp * cr + sy * sp * sr);
q.setX(cy * cp * sr - sy * sp * cr);
q.setY(sy * cp * sr + cy * sp * cr);
q.setZ(sy * cp * cr - cy * sp * sr);
return q;
}
void cloud_callback (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Create a container for the data.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*input, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
transform.setRotation(toQuaternion(0, -0.2565634, 0));
pcl_ros::transformPointCloud(*temp_cloud, *cloud_transformed, transform);
sensor_msgs::PointCloud2 cloud_publish;
pcl::toROSMsg(*cloud_transformed,cloud_publish);
cloud_publish.header = input->header;
pub.publish(cloud_publish);
}
int main (int argc, char** argv){
// Initialize ROS
ROS_INFO("Node started");
ros::init (argc, argv, "point_cloud_transform");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("/zed/point_cloud/cloud_registered", 3, cloud_callback);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("/zed/point_cloud/cloud_transformed", 3);
tf::TransformListener listener;
ros::spin();
return 0;
}
You can’t perform that action at this time.