Merge pull request #68 from sharronliu/master

grasp_ros2: fixed typo 'pcl_conversions'

Author: Sharron LIU

grasp_apps/random_pick/cfg/random_pick.yaml

@@ -23,10 +23,10 @@ GraspPlanner:
     grasp_score_threshold: 1
     grasp_frame_id: "base"
     grasp_approach: [0.0, 0.0, -1.0] # expect approaching in -z axis
-    grasp_approach_angle: 0.785 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle
-    grasp_offset: [0.000, -0.004, 0.000]
+    grasp_approach_angle: 0.6 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle
+    grasp_offset: [0.006, -0.003, 0.000]
     # grasp boundry in grasp_frame_id
-    grasp_boundry: [-0.25, 0.25, -0.73, -0.27, -0.15, 0.15]
-    eef_offset: 0.16
-    eef_yaw_offset: -0.7854 # M_PI/4
+    grasp_boundry: [-0.2, 0.2, -0.65, -0.30, -0.15, 0.15]
+    eef_offset: 0.154
+    eef_yaw_offset: 0.7854 # M_PI/4
     finger_joint_names: ["panda_finger_joint1", "panda_finger_joint2"]

grasp_apps/recognize_pick/cfg/recognize_pick.yaml

@@ -24,10 +24,10 @@ GraspPlanner:
     grasp_score_threshold: 1
     grasp_frame_id: "base"
     grasp_approach: [0.0, 0.0, -1.0] # expect approaching in -z axis
-    grasp_approach_angle: 0.785 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle
-    grasp_offset: [0.000, -0.004, 0.000]
+    grasp_approach_angle: 0.7 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle
+    grasp_offset: [0.006, -0.003, 0.000]
     # grasp boundry in grasp_frame_id
-    grasp_boundry: [-0.25, 0.25, -0.73, -0.27, -0.15, 0.15]
-    eef_offset: 0.16
-    eef_yaw_offset: -0.7854 # M_PI/4
+    grasp_boundry: [-0.2, 0.2, -0.65, -0.30, -0.15, 0.15]
+    eef_offset: 0.154
+    eef_yaw_offset: 0.7854 # M_PI/4
     finger_joint_names: ["panda_finger_joint1", "panda_finger_joint2"]

grasp_apps/recognize_pick/src/recognize_pick.cpp

@@ -114,6 +114,7 @@ int main(int argc, char **argv)
       robot_->moveToJointValues(joint_values_place, vel_, acc_);
       robot_->place(place_[0], place_[1], place_[2], place_[3], place_[4], place_[5], vel_, acc_, vscale_, appr_);
 #endif
+    rclcpp::spin_some(node_);
     place_pose_ = nullptr;
   }
 

grasp_ros2/src/grasp_detector_gpd.cpp

@@ -123,9 +123,6 @@ std::vector<Grasp> GraspDetectorGPD::detectGraspPosesInTopic()
   std::vector<Grasp> grasps;
 
   {
-    if (object_sub_) {
-      cloud_camera_->filterWorkspace(grasp_ws_);
-    }
     // preprocess the point cloud
     grasp_detector_->preprocessPointCloud(*cloud_camera_);
     // detect grasps in the point cloud
@@ -240,15 +237,15 @@ void GraspDetectorGPD::cloud_callback(const sensor_msgs::msg::PointCloud2::Share
               xyz.row(1).minCoeff(), xyz.row(1).maxCoeff(),
               xyz.row(2).minCoeff(), xyz.row(2).maxCoeff()};
             found = true;
-            filter1 = filter2;
+            cloud = filter2;
             break;
           }
         }
         if (!found) {return;}
       }
       if (filtered_pub_) {
         sensor_msgs::msg::PointCloud2 msg2;
-        pcl::toROSMsg(*filter1, msg2);
+        pcl::toROSMsg(*cloud, msg2);
         // workaround rviz rgba
         msg2.fields[3].name = "rgb";
         msg2.fields[3].datatype = 7;

grasp_ros2/tests/CMakeLists.txt

@@ -3,6 +3,7 @@ find_package(ament_cmake_gtest REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(grasp_msgs REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
 set(TEST_NAME tgrasp_ros2)
 
 ament_add_gtest(${TEST_NAME} tgrasp_ros2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/consts.cpp)
@@ -16,7 +17,7 @@ if(TARGET ${TEST_NAME})
     ${grasp_library_INCLUDE_DIRS}
   )
   ament_target_dependencies(${TEST_NAME}
-    pcl_conversion
+    pcl_conversions
     rclcpp
     grasp_msgs
     moveit_msgs