Moving a group of point clouds in rviz
Hi all, Thank you for your help beforehand. I am pretty new to PCL library so I apologize if my question seems easy. For my project, I need to show the point clouds of the environment to the operator in VR, and the operator should be able to pick and place a cylinder. I have written the code at the bottom of this message for object segmentation. So in VR, the operator should be able to move the cylinder's point clouds. My question is that after finding the cylinder: 1. How can I move the cylinder point clouds? 2. How can I merge the cylinder point clouds with the environment point clouds? Meaning that the operator would see the environment too( without the cylinder point cloud), while he/she is moving the cylinder.
As I am completely new to PCL, any comments would be appreciated so much. I look forward to your kind help!
#include <ros/ros.h>
#include<iostream>
#include <sensor_msgs/PointCloud2.h>
#include<pcl-1.8/pcl/point_cloud.h>
#include<pcl-1.8/pcl/point_types.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl-1.8/pcl/filters/extract_indices.h>
#include<pcl-1.8/pcl/filters/voxel_grid.h>
#include<pcl-1.8/pcl/filters/passthrough.h>
#include<pcl-1.8/pcl/segmentation/sac_segmentation.h>
#include<pcl-1.8/pcl/sample_consensus/method_types.h>
#include<pcl-1.8/pcl/sample_consensus/model_types.h>
#include<pcl-1.8/pcl/features/normal_3d.h>
#include<pcl-1.8/pcl/ModelCoefficients.h>
ros::Publisher pln_pub;
ros::Publisher cyl_pub;
//Everything happens in this call-back function
void cloud_cb(const pcl::PCLPointCloud2ConstPtr &input)
{
// All the data;
pcl::PCLPointCloud2::Ptr filtered(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr ex_pln(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr ex_cyl(new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> ::Ptr flt_pln(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr flt_cyl(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coeff_pln(new pcl::ModelCoefficients),coeff_cyl(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_pln(new pcl::PointIndices), inliers_cyl(new pcl::PointIndices);
// Objects needed
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> seg;
pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
pcl::ExtractIndices<pcl::Normal> extract_normal;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
// You can use the following line to convert PointCloud2 data to pcl::PointXYZ
pcl::fromPCLPointCloud2(*input,*input_cloud);
//estimate normals
ne.setSearchMethod(tree);
ne.setInputCloud(input_cloud);
ne.setKSearch(50);
ne.compute(*cloud_normals1);
// Here we create the segmentation object for the plane
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SacModel::SACMODEL_NORMAL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.03);
seg.setInputCloud(input_cloud);
seg.setInputNormals(cloud_normals1);
seg.segment(*inliers_pln,*coeff_pln);
// extracting the plane
extract.setInputCloud(input);
extract.setIndices(inliers_pln);
extract.setNegative(false);
extract.filter(*ex_pln);
pln_pub.publish(*ex_pln);
//removing the plane completely
extract.setNegative(true);
extract.filter(*filtered);
extract_normal.setNegative(true);
extract_normal.setInputCloud(cloud_normals1);
extract_normal.setIndices(inliers_pln);
extract_normal.filter(*cloud_normals2);
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*filtered,*xyz_filtered);
//from the new normals find what we are looking for
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SacModel::SACMODEL_CYLINDER);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(10000);
seg.setNormalDistanceWeight(0.1);
seg.setDistanceThreshold(0.05);
seg.setRadiusLimits(0,0.1);
seg.setInputCloud(xyz_filtered);
seg.setInputNormals(cloud_normals2);
seg.segment(*inliers_cyl,*coeff_cyl);
//extract the cylinder part
extract.setInputCloud(filtered);
extract.setIndices(inliers_cyl);
extract.setNegative(false);
extract.filter(*ex_cyl);
cyl_pub.publish(*ex_cyl);
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"seg_cylinder");
ros::NodeHandle n;
ros::Subscriber sub= n.subscribe("kinect2/sd/points",1,cloud_cb);
pln_pub=n.advertise<pcl::PCLPointCloud2>("PlanePC",1);
cyl_pub=n.advertise<pcl::PCLPointCloud2>("CylinderPC",1);
ros::spin();
}
Asked by Alireza_msb on 2021-02-12 02:43:03 UTC
Answers
I could solve the problem by changing my prespective a little bit. So instead of moving the pointClouds, we can creat a Marker which is consisted of many points which have the same color as the points of the pointcloud and then we can move or rotate the frame of the marker. Here's a code for making the marker ( If anyone has any other recommendation I would be very happy to hear that!!):
#include<ros/ros.h>
#include<visualization_msgs/Marker.h>
#include<pcl-1.8/pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl-1.8/pcl/point_types.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include<pcl-1.8/pcl/filters/passthrough.h>
ros::Publisher pub;
ros::Publisher pass_pub;
void cloud_cb(const pcl::PCLPointCloud2ConstPtr &input)
{
// Getting the target point cloud
pcl::PassThrough<pcl::PCLPointCloud2> pass;
pcl::PCLPointCloud2::Ptr flt_input(new pcl::PCLPointCloud2);
pass.setInputCloud(input);
pass.setFilterFieldName("x");
pass.setFilterLimits(0,2);
pass.filter(*flt_input);
pass.setInputCloud(flt_input);
pass.setFilterFieldName("y");
pass.setFilterLimits(-2,0);
pass.filter(*flt_input);
pass.setInputCloud(flt_input);
pass.setFilterFieldName("z");
pass.setFilterLimits(0,2);
pass.filter(*flt_input);
pass_pub.publish(*flt_input);
// Converting PointCloud2 to PointXYZRGBA
pcl::PointCloud<pcl::PointXYZRGBA> input_xyz;
pcl::fromPCLPointCloud2(*flt_input,input_xyz);
//create the marker
visualization_msgs::Marker marker;
marker.header.frame_id="/camera1";
marker.header.stamp=ros::Time::now();
marker.ns = "pcl_marker";
marker.id = 1;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
marker.scale.x = 0.004;
marker.scale.y = 0.004;
//marker.scale.z = 0.004;
for(size_t i=0; i<input_xyz.points.size(); i++) {
std_msgs::ColorRGBA c;
// getting the collor of points[i]
c.r = (float)input_xyz.points[i].r/256;
c.g = (float)input_xyz.points[i].g/256;
c.b = (float)input_xyz.points[i].b/256;
c.a=1;
geometry_msgs::Point p;
p.x = input_xyz.points[i].x;
p.y = input_xyz.points[i].y;
p.z = input_xyz.points[i].z;
marker.points.push_back(p);
marker.colors.push_back(c);
}
pub.publish(marker);
}
int main(int argc, char** argv)
{
ros::init(argc, argv,"kinect");
ros::NodeHandle n;
pub= n.advertise<visualization_msgs::Marker>("marker",10);
pass_pub= n.advertise<sensor_msgs::PointCloud2>("filteredPnt",1);
ros::Subscriber sub= n.subscribe("kinect2/sd/points",1,cloud_cb);
ros::spin();
}
Asked by Alireza_msb on 2021-02-13 06:30:18 UTC
Comments