机器人 抓取 Grasp Pose Detection (GPD) grasp类型转ROS的标准pose

flyfish

问题 ：原格式包含众多的Position，Orientation 是三个向量，应该怎么用哪个？

# This message describes a 2-finger grasp configuration by its 6-DOF pose, 
# consisting of a 3-DOF position and 3-DOF orientation, and the opening 
# width of the robot hand.

# 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

标准的Pose

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

3个向量就是3×3的矩阵，Point使用的top

geometry_msgs::msg::Pose  grasp_to_pose(gpd::GraspConfig & grasp)
{

  geometry_msgs::msg::Pose pose;
  tf2::Matrix3x3 orientation(
    grasp.approach.x, grasp.binormal.x, grasp.axis.x,
    grasp.approach.y, grasp.binormal.y, grasp.axis.y,
    grasp.approach.z, grasp.binormal.z, grasp.axis.z);

  tf2::Quaternion orientation_quat;
  orientation.getRotation(orientation_quat);
  orientation_quat.normalize();
  pose.orientation.x = orientation_quat.x();
  pose.orientation.y = orientation_quat.y();
  pose.orientation.z = orientation_quat.z();
  pose.orientation.w = orientation_quat.w();

  pose.position = grasp.top;

  return pose;
}