ros:两向量或位姿间最小旋转角度及旋向

描述:

输入:目标位姿,实时更新的当前位姿
功能:判断当前位姿----->目标位姿的最小旋转角度(rotation_angle)和旋转方向(rotation_dir)

代码

void target_reached(const geometry_msgs::PoseStamped &msg_target,const geometry_msgs::PoseStamped &msg_now)
  {
  
  //确定当前yaw和目标yaw(弧度制,0--->-pi;-pi---->0)
  tf::Quaternion quat,quat_now;
  tf::quaternionMsgToTF(msg_target.pose.orientation, quat);
  double roll, pitch, yaw_target, yaw_now;
  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw_target);

  tf::quaternionMsgToTF(msg_now.pose.orientation, quat_now);
  tf::Matrix3x3(quat_now).getRPY(roll, pitch, yaw_now);
  
  //确定旋转方向与旋转角度:正表示逆时针旋转,负表示应该顺时针旋转(弧度制)
  int rotation_dir;
  double rotation_angle;
  double yaw=yaw_target - yaw_now;
  if(yaw > (180*M_PI/180)){
    yaw = yaw - (360*M_PI/180);
  }
  else if(yaw < -(180*M_PI/180)){
    yaw = yaw + (360*M_PI/180);
  }
  rotation_dir =  (yaw>0)?1:-1;
  rotation_angle = fabs(yaw);
  }