通过向量叉乘计算姿态充电桩朝向

                          通过向量叉乘计算姿态充电桩朝向

需求：

通过geometry_msgs::PointStamped intersection_ros_line1; geometry_msgs::PointStamped intersection_ros_line2；geometry_msgs::PointStamped intersection_ros_point;其中 intersection_ros_line1与intersection_ros_point连成线，intersection_ros_line1起始端点，intersection_ros_point末尾端点。 intersection_ros_point与intersection_ros_line2连成线，intersection_ros_point起始端点intersection_ros_line2末尾端点。geometry_msgs::PointStamped mid 是 intersection_ros_line1与intersection_ros_line2的中心点， geometry_msgs::PointStamped mid 与 intersection_ros_point 连成直线作为姿态的x轴，通过这4个点计算图形的姿态，输出旋转矩阵及转成4元素。两线段的交点合方向朝向即为x正方向，geometry_msgs::PointStamped mid 与 intersection_ros_point 连成直线作为姿态的x轴，坐标系遵循右手法则，计算Z轴方向向量用（geometry_msgs::PointStamped mid 与 intersection_ros_point 向量与 intersection_ros_point与intersection_ros_line2向量叉乘得到）。计算Y轴方向向量 用Z轴方向向量与geometry_msgs::PointStamped mid 与 intersection_ros_point 向量叉乘得到。用Eigen库计算，此发布把结果通过tf1发布x=0.0，y=0.0,z=0.0,姿态用以上算出来的姿态。

需要进行以下步骤：

1. 定义几何信息的结构。
2. 计算 mid 点。
3. 计算旋转矩阵。
4. 计算四元数。
5. 使用 Eigen 库来进行矩阵计算。
6. 发布结果通过 tf1。

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>


// 计算 mid 点
//mid 点是 intersection_ros_line1 和 intersection_ros_line2 的中点。

geometry_msgs::PointStamped calculateMidPoint(const geometry_msgs::PointStamped& p1, const geometry_msgs::PointStamped& p2) {
    geometry_msgs::PointStamped mid;
    mid.point.x = (p1.point.x + p2.point.x) / 2.0;
    mid.point.y = (p1.point.y + p2.point.y) / 2.0;
    mid.point.z = (p1.point.z + p2.point.z) / 2.0;
    return mid;
}
//计算旋转矩阵，姿态
//需要计算一个旋转矩阵来表示新的坐标系。首先，我们需要计算 x, y, z 三个轴的向量。

Eigen::Matrix3d calculateRotationMatrix(const geometry_msgs::PointStamped& intersection_ros_line1, const geometry_msgs::PointStamped& intersection_ros_point, const geometry_msgs::PointStamped& mid, const geometry_msgs::PointStamped& intersection_ros_line2) {
    Eigen::Vector3d x_axis(intersection_ros_point.point.x - mid.point.x, intersection_ros_point.point.y - mid.point.y, intersection_ros_point.point.z - mid.point.z);
    Eigen::Vector3d z_axis = x_axis.cross(Eigen::Vector3d(intersection_ros_point.point.x - intersection_ros_line2.point.x, intersection_ros_point.point.y - intersection_ros_line2.point.y, intersection_ros_point.point.z - intersection_ros_line2.point.z));
    Eigen::Vector3d y_axis = z_axis.cross(x_axis);

    x_axis.normalize();
    y_axis.normalize();
    z_axis.normalize();

    Eigen::Matrix3d rotation_matrix;
    rotation_matrix << x_axis.x(), y_axis.x(), z_axis.x(),
                        x_axis.y(), y_axis.y(), z_axis.y(),
                        x_axis.z(), y_axis.z(), z_axis.z();
    return rotation_matrix;
}

//计算四元数
//从旋转矩阵中计算四元数以便后续使用。
Eigen::Quaterniond calculateQuaternion(const Eigen::Matrix3d& rotation_matrix) {
    Eigen::Quaterniond quaternion(rotation_matrix);
    return quaternion;
}

void publishTransform(const Eigen::Quaterniond& quaternion) {
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
    tf::Quaternion q(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "robot_frame"));
}
//在主函数中，我们初始化 ROS 节点，计算 mid 点、旋转矩阵和四元数，并定期发布这些数据。
int main(int argc, char** argv) {
    ros::init(argc, argv, "pose_calculator");
    ros::NodeHandle nh;

    geometry_msgs::PointStamped intersection_ros_line1;
    geometry_msgs::PointStamped intersection_ros_line2;
    geometry_msgs::PointStamped intersection_ros_point;

    // Initialize your points here
    // Example:
    // intersection_ros_line1.point.x = ...
    // intersection_ros_line1.point.y = ...
    // intersection_ros_line1.point.z = ...

    geometry_msgs::PointStamped mid = calculateMidPoint(intersection_ros_line1, intersection_ros_line2);
    Eigen::Matrix3d rotation_matrix = calculateRotationMatrix(intersection_ros_line1, intersection_ros_point, mid, intersection_ros_line2);
    Eigen::Quaterniond quaternion = calculateQuaternion(rotation_matrix);

    ros::Rate rate(10.0);
    while (nh.ok()) {
        publishTransform(quaternion);
        rate.sleep();
    }

    return 0;
}

详细解释

1. 计算中点 (calculateMidPoint): 通过求两个点的坐标平均值计算中点。
2. 计算旋转矩阵 (calculateRotationMatrix): 使用交叉乘积计算出坐标系的 z 轴，然后计算 y 轴，最后标准化这些向量并构建旋转矩阵。
3. 计算四元数 (calculateQuaternion): 使用 Eigen 库从旋转矩阵生成四元数。
4. 发布变换 (publishTransform): 使用 tf 库发布计算的四元数变换。
5. 主函数: 初始化 ROS 节点，计算中点、旋转矩阵和四元数，并定期发布这些数据。