#include
#include
#include
#include
void callback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
ROS_INFO("Message recieved:"); //msg to c string
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
float theta = M_PI/18; // The angle of rotation in radians
transform_1 (0,0) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
printf ("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl;
sensor_msgs::PointCloud2 op_cloud;
pcl_ros::transformPointCloud(transform_1,*cloud_msg,op_cloud);
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "cloud_trfm");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/velodyne_points",1,callback); //topic name, buffer size untill node can read, callback fn
ros::spin();
}
↧