From f238db47c42e4d5729895368a964e04c47124f5d Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Mon, 20 Apr 2020 19:34:50 -0400
Subject: [PATCH] Point Cloud & Start of MoveIt

- Add moveit include paths
- Add PointCloud implementation
---
 .../src/rrrobot/src/arm_controller_node.cpp   |  11 ++
 .../src/rrrobot/src/depth_camera_node.cpp     | 106 ++++++++++++++++--
 2 files changed, 105 insertions(+), 12 deletions(-)

diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
index 9feb701..700c618 100644
--- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
+++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
@@ -10,6 +10,17 @@
 #include <std_msgs/String.h>
 #include <std_srvs/Trigger.h>
 
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
 
 Class ArmController {
 public:
diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
index 524855f..8862759 100644
--- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
+++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
@@ -20,22 +20,104 @@
 #include <std_srvs/Trigger.h>
 
 
-pcl::PointCloud<pcl::PointXYZ> convert_pc(const sensor_msgs::PointCloud::ConstPtr & msg) {
-    // Convert from sensor_msgs/PointCloud.msg to necessary format
-    // Convert to PointCloud2
-    sensor_msgs::PointCloud2 cloud2 = new sensor_msgs::PointCloud2;
-    sensor_msgs::PointCloudToPointCloud2(msg, cloud2); 
+void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) {
+    // convert from PointCloud to PointCloud2
+	sensor_msgs::PointCloud2 cloud2;
+	sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2);	
 
-    // Convert to PCL data type
-    pcl::PointCloud<pcl::PointXYZ> output = new pcl::PointCloud<pcl::PointXYZ>;
-    pcl::fromROSMsg(cloud2, output);
-}
+	// Container for original & filtered data
+	pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
+	pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
+	pcl::PCLPointCloud2 cloud_filtered;
 
-void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) {
-    pcl::PointCloud<pcl::PointXYZ> cloud = convert_pc(msg);
+	// Convert to PCL data type
+	pcl_conversions::toPCL(cloud2, *cloud);
 
-    // extract clusters
+	// Perform the actual filtering
+	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+	sor.setInputCloud (cloudPtr);
+	sor.setLeafSize (0.02, 0.02, 0.02);
+	sor.filter (cloud_filtered);
 
+	pcl::PointCloud<pcl::PointXYZ> point_cloud;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::fromPCLPointCloud2( cloud_filtered, point_cloud);
+	pcl::copyPointCloud(point_cloud, *point_cloudPtr);
+
+    // Create the KdTree object for the search method of the extraction
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+	tree->setInputCloud(point_cloudPtr);
+
+	std::vector<pcl::PointIndices> cluster_indices;
+	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+	ec.setClusterTolerance(0.1); // 2cm
+	ec.setMinClusterSize(10); //100
+	ec.setMaxClusterSize(99000000);
+	ec.setSearchMethod(tree);
+	ec.setInputCloud(point_cloudPtr);
+	ec.extract(cluster_indices);
+
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+  int j= 0;
+ 
+  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+	  {
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+		    {
+                pcl::PointXYZRGB point;
+                point.x = point_cloudPtr->points[*pit].x;
+                point.y = point_cloudPtr->points[*pit].y;
+                point.z = point_cloudPtr->points[*pit].z;
+
+                if (j == 0) //Red	#FF0000	(255,0,0)
+			     {
+				      point.r = 0;
+				      point.g = 0;
+				      point.b = 255;
+			     }
+			    else if (j == 1) //Lime	#00FF00	(0,255,0)
+			     {
+				      point.r = 0;
+				      point.g = 255;
+				      point.b = 0;
+			     }
+			    else if (j == 2) // Blue	#0000FF	(0,0,255)
+			     {
+				      point.r = 255;
+				      point.g = 0;
+				      point.b = 0;
+			     }
+			    else
+		   	     {
+				      if (j % 2 == 0)
+				       {
+					        point.r = 255 * j / (cluster_indices.size());
+					        point.g = 128;
+					        point.b = 50;
+				       }
+				      else
+				       {
+					        point.r = 0;
+					        point.g = 255 * j / (cluster_indices.size());
+					        point.b = 128;
+				       }
+                 }
+                point_cloud_segmented->push_back(point);
+			
+            }
+        j++;
+    }
+  
+    // Convert to ROS data type
+    point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id;
+    if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered);
+    else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered);
+    sensor_msgs::PointCloud2 output;
+    pcl_conversions::fromPCL(cloud_filtered, output);
+
+    // Publish the data
+    pub.publish (output);
 }
 
 int main(int argc, char ** argv) {