机器人 检测点云中的抓取姿势 Grasp Pose Detection (GPD) 扩展实例编写_approach、binormal、axis

机器人 检测点云中的抓取姿势 Grasp Pose Detection (GPD) 扩展实例编写

flyfish

环境
Ubuntu16.04
ROS kinetic
Detect Grasps With an RGBD camera
使用RGBD摄像机检测抓取
语言:C++
相机：astra

在CmakeLists中添加：

add_compile_options(-std=c++11)

add_executable(${PROJECT_NAME}_get_grasps src/interface/get_grasps.cpp)

target_link_libraries(${PROJECT_NAME}_get_grasps
                      ${catkin_LIBRARIES})

set_target_properties(${PROJECT_NAME}_get_grasps
                      PROPERTIES OUTPUT_NAME get_grasps
                      PREFIX "")
1
2
3
4
5
6
7
8
9
10

实例代码 有些多余的头文件

//std
#include <tuple>
#include <map>
#include <vector>
#include <queue>
#include <mutex>
#include <string>

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/filters/random_sample.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// ROS

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/MarkerArray.h>

#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Empty.h>
#include <std_srvs/SetBool.h>
#include <std_msgs/Int64.h>
#include <std_msgs/Empty.h>
// this project (services)

// GPG
#include <gpg/cloud_camera.h>

// this project (messages)
#include <gpd/CloudIndexed.h>
#include <gpd/CloudSamples.h>
#include <gpd/CloudSources.h>
#include <gpd/GraspConfig.h>
#include <gpd/GraspConfigList.h>
#include <gpd/SamplesMsg.h>
#include <gpd/detect_grasps.h>

//tf
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
//moveit  

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/ompl_interface/ompl_interface.h>
#include <moveit/dynamics_solver/dynamics_solver.h>




//action
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>


#include <dynamic_reconfigure/server.h>


//GraspConfigList的解释
// # This message stores a list of grasp configurations.

// # The time of acquisition, and the coordinate frame ID.
// Header header

// # The list of grasp configurations.
// gpd/GraspConfig[] grasps


// # Position
// geometry_msgs/Point bottom # centered bottom/base of the robot hand)
// geometry_msgs/Point top # centered top/fingertip of the robot hand)
// geometry_msgs/Point surface # centered position on object surface

// # Orientation represented as three axis (R = [approach binormal axis])
// geometry_msgs/Vector3 approach # The grasp approach direction
// geometry_msgs/Vector3 binormal # The binormal
// geometry_msgs/Vector3 axis # The hand axis

// geometry_msgs/Point sample # Point at which the grasp was found

// std_msgs/Float32 width # Required aperture (opening width) of the robot hand

// std_msgs/Float32 score # Score assigned to the grasp by the classifier
void detect_grasps_callback(const gpd::GraspConfigList& l)
{
  // 将此消息转换成moveit能够识别的消息
  gpd::GraspConfig g = l.grasps[0];

  //将自定义形式转换成标准的多种形式输出

  double coefficient = M_PI / 180;
  double roll, pitch, yaw;

// File: geometry_msgs/Vector3.msg
// Raw Message Definition
// # This represents a vector in free space. 
// # It is only meant to represent a direction. Therefore, it does not
// # make sense to apply a translation to it (e.g., when applying a 
// # generic rigid transformation to a Vector3, tf2 will only apply the
// # rotation). If you want your data to be translatable too, use the
// # geometry_msgs/Point message instead.

// float64 x
// float64 y
// float64 z


tf::Matrix3x3 m(
g.approach.x, g.binormal.x, g.axis.x, 
g.approach.y, g.binormal.y, g.axis.y, 
g.approach.z, g.binormal.z, g.axis.z); 


            m.getEulerYPR(yaw, pitch, roll); //RPY提供给人肉眼看


            std::cout << m.getRow(0).getX() << m.getRow(0).getY() << m.getRow(0).getZ() << std::endl;
            std::cout << m.getRow(1).getX() << m.getRow(1).getY() << m.getRow(1).getZ() << std::endl;
            std::cout << m.getRow(2).getX() << m.getRow(2).getY() << m.getRow(2).getZ() << std::endl;


            geometry_msgs::Point point;
            point.x = g.sample.x;
            point.y = g.sample.y;
            point.z = g.sample.z;
            //输出x,y,z，r,p,y
             ROS_INFO("output xyzrpy: %f  %f  %f  %f  %f  %f", point.x ,point.y ,point.z, roll, pitch, yaw);

            geometry_msgs::PoseStamped target_pose_stamped; //四元数提供给计算机使用
            target_pose_stamped.pose.position.x = point.x;
            target_pose_stamped.pose.position.y = point.y;
            target_pose_stamped.pose.position.z = point.z;
            target_pose_stamped.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
}

int main(int argc, char *argv[])
{
  const std::string FRAME = "world";

  ros::init(argc, argv, "get_grasps"); //初始化ROS节点
  ros::NodeHandle n; //创建节点句柄

  ros::Subscriber sub = n.subscribe("/detect_grasps/clustered_grasps", 1000 , detect_grasps_callback);

  ros::Rate rate(1);
  while (ros::ok())
  {
    ros::spinOnce();
    rate.sleep();
  }
  

  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162

最后是启动
1启动相机
roslaunch astra_launch astra.launch

2启动rviz
rosrun rviz rviz

3 rviz中加载配置文件
gpd/tutorials/openni2.rviz.

4 启动ROS节点 检测夹抓
roslaunch gpd tutorial1.launch

5启动自定义节点
rosrun gpd get_grasps

6 输出结果

tutorial1 输出结果

==== Selected grasps ====
Grasp 0: -5757.96
Grasp 1: -5781.67
Grasp 2: -6231.01
Grasp 3: -6646.38
Grasp 4: -6646.38
[ INFO] [1545795935.616926176]: Selected the 5 highest scoring grasps.
[ INFO] [1545795935.616977212]: Published 5 highest-scoring grasps.
[ INFO] [1545795935.617042676]: Waiting for point cloud to arrive ...
[ INFO] [1545795935.635950098]: Received cloud with 307200 points.
Processing cloud with: 307200 points.
Calculating surface normals ...
Using integral images for surface normals estimation ...
 runtime (normals): 0.113144
Reversing direction of normals that do not point to at least one camera ...
 reversed 206577 normals
 runtime (reverse normals): 0.00747134
After workspace filtering: 102965 points left.
After voxelization: 38100 points left.
Subsampled 100 at random uniformly.
Estimating local reference frames ...
Estimated 100 local reference frames in 0.0043785 sec.
Finding hand poses ...
 Found 12 grasp candidate sets in 0.527498 sec.
====> HAND SEARCH TIME: 0.541366
[ INFO] [1545795936.337648733]: Generated 12 grasp candidate sets.
Creating grasp images for classifier input ...
time for computing 12 point neighborhoods with 4 threads: 0.0397289s
 Image creation time: 0.625206
 image_list: 96, valid_images: 28, valid_grasps: 28
 Prediction time: 1.04166
Selecting the 5 highest scoring grasps ...
 grasp #0, score: -2354.93, -2354.93
 grasp #1, score: -4028.55, -4028.55
 grasp #2, score: -4255.32, -4255.32
 grasp #3, score: -4424.9, -4424.9
 grasp #4, score: -5083.04, -5083.04
Total classification time: 1.04174
[ INFO] [1545795937.379457995]: Selected 5 valid grasps after predicting their scores.
[ INFO] [1545795937.379486942]: Found 0 clusters.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

get_grasps 输出结果

0.802892-0.4522060.388426
-0.2570310.3252990.910008
-0.537866-0.8304760.144949
[ INFO] [1545792533.066958774]: output xyzrpy: 0.266088  0.157333  0.677000  -1.398000  0.567903  -0.309823
1
2
3
4

更好的查看类型

rossrv show detect_grasps
[gpd/detect_grasps]:
gpd/CloudIndexed cloud_indexed
  gpd/CloudSources cloud_sources
    sensor_msgs/PointCloud2 cloud
      std_msgs/Header header
        uint32 seq
        time stamp
        string frame_id
      uint32 height
      uint32 width
      sensor_msgs/PointField[] fields
        uint8 INT8=1
        uint8 UINT8=2
        uint8 INT16=3
        uint8 UINT16=4
        uint8 INT32=5
        uint8 UINT32=6
        uint8 FLOAT32=7
        uint8 FLOAT64=8
        string name
        uint32 offset
        uint8 datatype
        uint32 count
      bool is_bigendian
      uint32 point_step
      uint32 row_step
      uint8[] data
      bool is_dense
    std_msgs/Int64[] camera_source
      int64 data
    geometry_msgs/Point[] view_points
      float64 x
      float64 y
      float64 z
  std_msgs/Int64[] indices
    int64 data
---
gpd/GraspConfigList grasp_configs
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  gpd/GraspConfig[] grasps
    geometry_msgs/Point bottom
      float64 x
      float64 y
      float64 z
    geometry_msgs/Point top
      float64 x
      float64 y
      float64 z
    geometry_msgs/Point surface
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 approach
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 binormal
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 axis
      float64 x
      float64 y
      float64 z
    geometry_msgs/Point sample
      float64 x
      float64 y
      float64 z
    std_msgs/Float32 width
      float32 data
    std_msgs/Float32 score
      float32 data
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

PARAMETERS

 * /detect_grasps/angle_thresh: 0.1
 * /detect_grasps/camera_position: [0, 0, 0]
 * /detect_grasps/cloud_topic: /camera/depth_reg...
 * /detect_grasps/cloud_type: 0
 * /detect_grasps/create_image_batches: False
 * /detect_grasps/filter_grasps: False
 * /detect_grasps/filter_half_antipodal: False
 * /detect_grasps/filter_table_side_grasps: False
 * /detect_grasps/finger_width: 0.01
 * /detect_grasps/hand_depth: 0.06
 * /detect_grasps/hand_height: 0.02
 * /detect_grasps/hand_outer_diameter: 0.12
 * /detect_grasps/image_depth: 0.06
 * /detect_grasps/image_height: 0.02
 * /detect_grasps/image_num_channels: 15
 * /detect_grasps/image_outer_diameter: 0.1
 * /detect_grasps/image_size: 60
 * /detect_grasps/init_bite: 0.01
 * /detect_grasps/lenet_params_dir: /home/pumpkinking...
 * /detect_grasps/max_aperture: 0.072
 * /detect_grasps/min_aperture: 0.029
 * /detect_grasps/min_inliers: 1
 * /detect_grasps/min_score_diff: 0
 * /detect_grasps/nn_radius: 0.01
 * /detect_grasps/num_orientations: 8
 * /detect_grasps/num_samples: 100
 * /detect_grasps/num_selected: 5
 * /detect_grasps/num_threads: 4
 * /detect_grasps/plot_candidates: False
 * /detect_grasps/plot_clusters: False
 * /detect_grasps/plot_filtered_grasps: False
 * /detect_grasps/plot_normals: False
 * /detect_grasps/plot_samples: False
 * /detect_grasps/plot_selected_grasps: False
 * /detect_grasps/plot_valid_grasps: False
 * /detect_grasps/remove_outliers: False
 * /detect_grasps/remove_plane_before_image_calculation: False
 * /detect_grasps/rviz_topic: grasps_rviz
 * /detect_grasps/samples_topic: 
 * /detect_grasps/table_height: 0.0
 * /detect_grasps/table_thresh: 0.05
 * /detect_grasps/use_importance_sampling: False
 * /detect_grasps/vertical_axis: [0, 0, 1]
 * /detect_grasps/voxelize: True
 * /detect_grasps/workspace: [-1, 1, -1, 1, -1...
 * /detect_grasps/workspace_grasps: [0.55, 1.0, -0.41...
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48