Merge branch 'GEAR' of github.com:EECS-467-W20-RRRobot-Project/RRRobot into GEAR

.gitignore

@@ -1,9 +1,9 @@
 # Don't track the content of these folders #
 ############################################
-rrrobot_ws/lib/
-rrrobot_ws/build
-rrrobot_ws/devel
-rrrobot_ws/install
+src/*/lib/
+src/*/build
+src/*/devel
+src/*/install
 .*
 !.gitignore
 

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "src/ariac_ws/src/universal_robot"]
+	path = src/ariac_ws/src/universal_robot
+	url = https://github.com/osrf/universal_robot
+	branch = ariac_2019_ur10_moveit_config

docker_env/docker-compose.yml

@@ -60,7 +60,7 @@ services:
         # Mount gazebo folder on host to app folder in container
         volumes:
             #- .docker_env/gazebo/.gazebo:/root/.gazebo
-            - ../rrrobot_ws:/app/rrrobot_ws
+            - ../src/rrrobot_ws:/app/rrrobot_ws
         # Set DISPLAY variable and network mode for GUIs
         environment:
             - DISPLAY=${IP_ADDRESS}:0.0
@@ -77,12 +77,13 @@ services:
         build: ./gear
         # Mount gear folder on host to app folder in container
         volumes:
-            - ../rrrobot_ws:/app/rrrobot_ws
+            - ../src/rrrobot_ws:/app/rrrobot_ws
+            - ../src/ariac_ws:/app/ariac_ws
         # Set DISPLAY variable and network mode for GUIs
         environment:
             - DISPLAY=${IP_ADDRESS}:0.0
         # Set working directory in container to app folder
-        working_dir: /app/rrrobot_ws
+        working_dir: /app
         hostname: "rrrobot-env"
         networks:
             - ros

rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -1,57 +0,0 @@
-// depth_camera_node.cpp
-
-#include <algorithm>
-#include <vector>
-#include <string>
-
-#include <ros/ros.h>
-#include "sensor_msgs/PointCloud.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "message_filters/subscriber.h"
-#include "pcl/io/pcd_io.h"
-#include "pcl/point_types.h"
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/segmentation/extract_clusters.h>
-
-#include <std_msgs/Float32.h>
-#include <std_msgs/String.h>
-#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); 
-
-    // Convert to PCL data type
-    pcl::PointCloud<pcl::PointXYZ> output = new pcl::PointCloud<pcl::PointXYZ>;
-    pcl::fromROSMsg(cloud2, output);
-}
-
-void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) {
-    pcl::PointCloud<pcl::PointXYZ> cloud = convert_pc(msg);
-
-    // extract clusters
-
-}
-
-int main(int argc, char ** argv) {
-    // TODO: Subscribe to depth camera topic
-    // Last argument is the default name of the node.
-    ros::init(argc, argv, "depth_camera_node");
-
-    ros::NodeHandle node;
-    
-    ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
-
-    
-    
-    // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
-    
-
-    // TODO: Publish object's current location to topic for RRRobot node to listen to
-    
-}

src/rrrobot_ws/src/CMakeLists.txt

@@ -0,0 +1,67 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching fot catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()

src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml

@@ -7,5 +7,5 @@ sensors:
     depth_camera_1:
       type: depth_camera
       pose:
-        xyz: [1.2, 0.75, 1.5]
-        rpy: [0, 1.5707, 0]
+        xyz: [0.75, 0.75, 1.5]
+        rpy: [0, 1, 0]

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:

src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -0,0 +1,139 @@
+// depth_camera_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+
+#include <ros/ros.h>
+#include "sensor_msgs/PointCloud.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "message_filters/subscriber.h"
+#include "pcl/io/pcd_io.h"
+#include "pcl/point_types.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+
+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);	
+
+	// Container for original & filtered data
+	pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
+	pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
+	pcl::PCLPointCloud2 cloud_filtered;
+
+	// Convert to PCL data type
+	pcl_conversions::toPCL(cloud2, *cloud);
+
+	// 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) {
+    // TODO: Subscribe to depth camera topic
+    // Last argument is the default name of the node.
+    ros::init(argc, argv, "depth_camera_node");
+
+    ros::NodeHandle node;
+    
+    ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
+
+    
+    
+    // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
+    
+
+    // TODO: Publish object's current location to topic for RRRobot node to listen to
+    
+}