From f4f38544ae5a3e872d1efe731c918da6c4ac9a0f Mon Sep 17 00:00:00 2001 From: dmaccaline Date: Thu, 25 May 2023 11:32:31 -0400 Subject: [PATCH 1/9] modified pcl required version --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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}) From 129a83430904b48d8a760b1eff168d684b51a8d3 Mon Sep 17 00:00:00 2001 From: dmaccaline Date: Thu, 25 May 2023 11:33:05 -0400 Subject: [PATCH 2/9] removed lines so that service would function properly --- src/gpd_ros/grasp_detection_server.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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; From bbff7f0d10645b91671d336fff56688996549321 Mon Sep 17 00:00:00 2001 From: dmaccaline Date: Thu, 25 May 2023 11:58:49 -0400 Subject: [PATCH 3/9] added launch files for gpd services --- launch/serv.launch | 21 +++++++++++++++++++++ launch/serv_vertical.launch | 22 ++++++++++++++++++++++ 2 files changed, 43 insertions(+) create mode 100644 launch/serv.launch create mode 100644 launch/serv_vertical.launch diff --git a/launch/serv.launch b/launch/serv.launch new file mode 100644 index 0000000..133c0d3 --- /dev/null +++ b/launch/serv.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + diff --git a/launch/serv_vertical.launch b/launch/serv_vertical.launch new file mode 100644 index 0000000..0714bda --- /dev/null +++ b/launch/serv_vertical.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + From d3c86918d9cd57c7a62aa784587cbf169bcb94be Mon Sep 17 00:00:00 2001 From: dmaccaline Date: Thu, 25 May 2023 11:59:33 -0400 Subject: [PATCH 4/9] added additional readme for additional install instructions --- READMEUpdated.md | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 READMEUpdated.md diff --git a/READMEUpdated.md b/READMEUpdated.md new file mode 100644 index 0000000..29ac29e --- /dev/null +++ b/READMEUpdated.md @@ -0,0 +1,28 @@ +# Grasp Pose Detection (GPD) + +## Required Modifications for GPD to compile and run on different machines: +- In gpd/cfg, the files ros_eigen_params.cfg and ros_eigen_params_vertical.cfg + - The wights file path must be updated to match the file path on your system (line 35) + - Additionally, the robot hand geometry (lines 7 - 20) must be changed if the class is being used on a robot other than Fetch. +- 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 + +## Required Modifications in order to create different detectors with different 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 - false, 1 is true + + +### If experiencing difficulty: +- 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 compilation issues relating to Eigen are found:\ + - Check Eigen3 installation, + - If the problem persists, change all includes with #include to #include + - (whether this is needed depends on how eigen was installed) \ No newline at end of file From 48811ebcc31a1a17cace643c9af4a9be09a5679d Mon Sep 17 00:00:00 2001 From: dmaccaline Date: Thu, 25 May 2023 15:31:40 -0400 Subject: [PATCH 5/9] updated READMEUpdated with additional information on installing gpd and gpd_ros --- READMEUpdated.md | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/READMEUpdated.md b/READMEUpdated.md index 29ac29e..d820e82 100644 --- a/READMEUpdated.md +++ b/READMEUpdated.md @@ -7,7 +7,7 @@ - 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 -## Required Modifications in order to create different detectors with different config files: +## 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 @@ -16,13 +16,9 @@ To create different versions of the gpd_detect service: - 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 - false, 1 is true + - The comments in the config file explains what each parameter does. Note that 0 is false, 1 is true -### If experiencing difficulty: +### 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 compilation issues relating to Eigen are found:\ - - Check Eigen3 installation, - - If the problem persists, change all includes with #include to #include - - (whether this is needed depends on how eigen was installed) \ No newline at end of file + - 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 \ No newline at end of file From d00f760a977841a226a1c31699f2a8cd95393ffe Mon Sep 17 00:00:00 2001 From: csrobot Date: Wed, 31 May 2023 14:39:54 -0400 Subject: [PATCH 6/9] updated README with information on each launch file and potential issues --- READMEUpdated.md | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/READMEUpdated.md b/READMEUpdated.md index d820e82..db69105 100644 --- a/READMEUpdated.md +++ b/READMEUpdated.md @@ -4,9 +4,23 @@ - In gpd/cfg, the files ros_eigen_params.cfg and ros_eigen_params_vertical.cfg - The wights file path must be updated to match the file path on your system (line 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 @@ -21,4 +35,8 @@ To create different versions of the gpd_detect service: ### 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 \ No newline at end of file + - 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 From b2765eb2c5e318792ae8e66178b1a18a88ae52b9 Mon Sep 17 00:00:00 2001 From: csrobot Date: Wed, 31 May 2023 14:40:31 -0400 Subject: [PATCH 7/9] added launch file for differing init_bite config --- launch/serv_large_vertical.launch | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 launch/serv_large_vertical.launch 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 @@ + + + + + + + + + + + + + + + + + + + + From 4fedbc2348b2d4986df459af5895d1f9742ebaf0 Mon Sep 17 00:00:00 2001 From: csrobot Date: Wed, 31 May 2023 14:41:07 -0400 Subject: [PATCH 8/9] modified file paths --- launch/serv.launch | 2 +- launch/serv_vertical.launch | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/serv.launch b/launch/serv.launch index 133c0d3..f23506f 100644 --- a/launch/serv.launch +++ b/launch/serv.launch @@ -12,7 +12,7 @@ - + diff --git a/launch/serv_vertical.launch b/launch/serv_vertical.launch index 0714bda..d3fcf69 100644 --- a/launch/serv_vertical.launch +++ b/launch/serv_vertical.launch @@ -13,7 +13,7 @@ - + From cd9e0add97a8003da1e5d470e273e7e76703c7e4 Mon Sep 17 00:00:00 2001 From: csrobot Date: Wed, 31 May 2023 14:44:47 -0400 Subject: [PATCH 9/9] updated list of files that need a filepath modification --- READMEUpdated.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/READMEUpdated.md b/READMEUpdated.md index db69105..a4710e7 100644 --- a/READMEUpdated.md +++ b/READMEUpdated.md @@ -1,8 +1,8 @@ # Grasp Pose Detection (GPD) ## Required Modifications for GPD to compile and run on different machines: -- In gpd/cfg, the files ros_eigen_params.cfg and ros_eigen_params_vertical.cfg - - The wights file path must be updated to match the file path on your system (line 35) +- 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