diff --git a/CMakeLists.txt b/CMakeLists.txt
index 313765f..a1df99d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,8 @@ project(gpd_ros)
find_package(catkin REQUIRED COMPONENTS cmake_modules eigen_conversions message_generation roscpp rospy sensor_msgs std_msgs)
# PCL
-find_package(PCL 1.9 REQUIRED)
+#find_package(PCL 1.9 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
diff --git a/READMEUpdated.md b/READMEUpdated.md
new file mode 100644
index 0000000..a4710e7
--- /dev/null
+++ b/READMEUpdated.md
@@ -0,0 +1,42 @@
+# Grasp Pose Detection (GPD)
+
+## Required Modifications for GPD to compile and run on different machines:
+- In gpd/cfg, the files ros_eigen_params.cfg, ros_eigen_params_vertical.cfg, and ros_eigen_params_verticl_large_bite.cfg
+ - The wights file path must be updated to match the file path on your system (line 32-35)
+ - Additionally, the robot hand geometry (lines 7 - 20) must be changed if the class is being used on a robot other than Fetch.
+- Make sure gpd is installed as a library, by using the sudo make install command, as specified in the gpd README.md
+- In each launch file in gpd_ros
+ - The config file must be updated to point to a config file, such as the ones modified in gpd/cfg
+
+## Using the gpd_ros services
+- Each launch file starts a different service.
+ - serv.launch starts a basic service, with a low init_bite value, and a no restictions on orientation
+ - init_bite is the amount of the object that must be inside the robot's gripper
+ - serv_vertical.launch launches an identical service, but with a restriction on orientation. All grasps from this service will be within 45 degrees of the z (vertical) axis
+ - serv_large_verticle.lainch startes a service identical to serv_verticle, but with a larger init_bite
+ - A larger init bite helps the robot grasp larger objects, as more of the object is in the robot's hand, giving it a better grip
+ - Using too large of an init_bite on a small object can cause no grasps to be found, as such, which one is used should depend on the object size
+- The ur5.launch file included launches a node that can detect grasps
+ - This topic waits for a message on the topic specified in the launch file and publishes the result to another topic
+ - This version is not recommended due to needing to publish to one topic and subscribe for a result, as opposed to the service format
+ - This launch file can be used to test the different types of messages that can be passed to gpd. The node can accept a normal pointcloud, a cloud samples message, or a cloud indexed message, however the services can only use cloud indexed messages
+
+## Required Modifications in order to create Additional detectors and Additional config files:
+To create different versions of the gpd_detect service:
+- A new launch file should be made
+ - The format of serv.launch can be copied and modified
+ - The node name must be modified and unique
+ - The config file should be changed to the configuration desired for this instance of the gpd detector
+ - Optionally the rviz_topic value field can be changed to make the detector publish to a different topic
+- A config file should be created
+ - The format of ros_eigen_params.cfg (in gpd/cfg) can be copied and modified
+ - The comments in the config file explains what each parameter does. Note that 0 is false, 1 is true
+
+
+### If the GPD service crashes when processing a request:
+- If the detector crashes after a request is made, re-start the detector and carefully look over the output on startup. Specifically, look for lines saying if a file could not be found.
+ - Note that these lines are not printed in a way to distinguish them from other output lines, and will not cause any issues on startup, but will cause the detector to crash when a request is made
+- If the GPD crashes after printing the line "Received cloud with x points, and y samples
+ Processing cloud with z points.", ensure that Nans are removed from the pointcloud of both the scene and object before passing them to gpd. (use pcl::removeNaNFromPointCloud)
+ - GPD does use a remove Nan function, however this function can cause the service to crash when Nans are present
+ - Function is in gpd/src/gpd/util/cloud.cpp, named Cloud::removeNans(), and crashes on the line eifilter.filter(*cloud_processed_);
\ No newline at end of file
diff --git a/launch/serv.launch b/launch/serv.launch
new file mode 100644
index 0000000..f23506f
--- /dev/null
+++ b/launch/serv.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/serv_large_vertical.launch b/launch/serv_large_vertical.launch
new file mode 100644
index 0000000..8f3348a
--- /dev/null
+++ b/launch/serv_large_vertical.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/serv_vertical.launch b/launch/serv_vertical.launch
new file mode 100644
index 0000000..d3fcf69
--- /dev/null
+++ b/launch/serv_vertical.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/gpd_ros/grasp_detection_server.cpp b/src/gpd_ros/grasp_detection_server.cpp
index 171b6d9..1b6cda4 100644
--- a/src/gpd_ros/grasp_detection_server.cpp
+++ b/src/gpd_ros/grasp_detection_server.cpp
@@ -5,11 +5,12 @@ GraspDetectionServer::GraspDetectionServer(ros::NodeHandle& node)
{
cloud_camera_ = NULL;
+ /*
// set camera viewpoint to default origin
std::vector camera_position;
node.getParam("camera_position", camera_position);
view_point_ << camera_position[0], camera_position[1], camera_position[2];
-
+*/
std::string cfg_file;
node.param("config_file", cfg_file, std::string(""));
grasp_detector_ = new gpd::GraspDetector(cfg_file);
@@ -111,6 +112,9 @@ bool GraspDetectionServer::detectGrasps(gpd_ros::detect_grasps::Request& req, gp
// Publish the detected grasps.
gpd_ros::GraspConfigList selected_grasps_msg = GraspMessages::createGraspListMsg(grasps, cloud_camera_header_);
+ for(int i = 0; i < 5; i++) {
+ grasps_pub_.publish(selected_grasps_msg);
+ }
res.grasp_configs = selected_grasps_msg;
ROS_INFO_STREAM("Detected " << selected_grasps_msg.grasps.size() << " highest-scoring grasps.");
return true;