旋转矩阵 (Rotation Matrix) 详解与 ROS 实践

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "rotation_publisher");
    ros::NodeHandle nh;

    tf2_ros::StaticTransformBroadcaster static_broadcaster;
    geometry_msgs::TransformStamped static_transformStamped;

    // 1. 设置父子关系：s -> b
    static_transformStamped.header.stamp = ros::Time::now();
    static_transformStamped.header.frame_id = "s";
    static_transformStamped.child_frame_id = "b";

    // 2. 平移（纯旋转设为 0，此处设 x=-2 仅用于 RViz 视觉区分）
    static_transformStamped.transform.translation.x = -2.0;
    static_transformStamped.transform.translation.y = 0.0;
    static_transformStamped.transform.translation.z = 0.0;

    // 3. 旋转：{b} 由 {s} 绕 z_s 轴旋转 90° 得到
    tf2::Vector3 z_axis(0, 0, 1);
    double theta = M_PI / 2.0;
    tf2::Quaternion quat;
    quat.setRotation(z_axis, theta);

    static_transformStamped.transform.rotation.x = quat.x();
    static_transformStamped.transform.rotation.y = quat.y();
    static_transformStamped.transform.rotation.z = quat.z();
    static_transformStamped.transform.rotation.w = quat.w();

    static_broadcaster.sendTransform(static_transformStamped);
    ROS_INFO("Published transform: s -> b (Rot z by 90 deg)");
    ros::spin();
    return 0;
}

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/bind.hpp>

void transformPoint(const tf2_ros::Buffer& buffer) {
    // 1. 定义在 {b} 系中的点
    geometry_msgs::PointStamped p_b;
    p_b.header.frame_id = "b";
    p_b.header.stamp = ros::Time(0);
    p_b.point.x = -1.0;
    p_b.point.y =  0.0;
    p_b.point.z =  0.0;

    try {
        // 2. 获取从 {b} 到 {s} 的变换 (target_frame, source_frame)
        geometry_msgs::TransformStamped R_sb = buffer.lookupTransform("s", "b", ros::Time(0));

        // 3. 执行坐标变换: p_s = R_sb * p_b
        geometry_msgs::PointStamped p_s;
        tf2::doTransform(p_b, p_s, R_sb);

        ROS_INFO("{b}:(%.2f, %.2f, %.2f) ---> {s}:(%.2f, %.2f, %.2f)",
                 p_b.point.x, p_b.point.y, p_b.point.z,
                 p_s.point.x, p_s.point.y, p_s.point.z);
    } catch (tf2::TransformException &ex) {
        ROS_ERROR("TF Lookup failed: %s", ex.what());
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "rotation_listener");
    ros::NodeHandle nh;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    // 定时执行 (1Hz)
    ros::Timer timer = nh.createTimer(ros::Duration(1.0), 
                                      boost::bind(&transformPoint, boost::ref(tfBuffer)));
    ros::spin();
    return 0;
}