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

需求：

通过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,姿态用以上算出来的。

需要进行以下步骤：

定义几何信息的结构。
计算 mid 点。
计算旋转矩阵。
计算四元数。
使用 Eigen 库来进行矩阵计算。
发布结果通过 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;
}

详细解释

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