ROS工程实践3—tf_node节点参数设置

tf_node节点

tf_node.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/PointCloud2.h>

#define PI 3.141592653589

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "tf_publisher");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);

    tf::TransformBroadcaster broadcaster;
    tf::Transform lidar_to_base;

    tf::Quaternion q;
    q.setRPY(0, 0, (-PI + 0.075) / 2);
    lidar_to_base.setRotation(q);
    lidar_to_base.setOrigin(tf::Vector3(0, -2.975, 0));

    while (nh.ok()) {
        broadcaster.sendTransform(
            tf::StampedTransform(lidar_to_base, ros::Time::now(),
            "Pandar40P",
            "base_link"));
        char out_msg[1024];
        sprintf(out_msg, "lidar_to_vcs: R(W: %f, X: %f, Y: %f, Z: %f); T(%f, %f, %f)",
                         lidar_to_base.getRotation().getW(),
                         lidar_to_base.getRotation().getX(),
                         lidar_to_base.getRotation().getY(),
                         lidar_to_base.getRotation().getZ(),
                         lidar_to_base.getOrigin().getX(),
                         lidar_to_base.getOrigin().getY(),
                         lidar_to_base.getOrigin().getZ());
        ROS_INFO("%s",out_msg);
        
        loop_rate.sleep();
    }
}

tf.cpp

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_publisher");
    ros::NodeHandle n;
    ros::Rate r(100);
    tf::TransformBroadcaster broadcaster;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.1, 0, 0.2));
    tf::Quaternion q;
    q.setRPY(0, 0, 0);
    transform.setRotation(q);
    while(n.ok()) {
        broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));  //发布了base_link和base_laser之间的坐标关系
        r.sleep();
    }

    return 0;
}

启动节点：

source ./devel/setup.sh

rosrun tf tf_node

节点说明：

base_link是父坐标系；

base_laser是子坐标系；

1.setRPY；

这个是绕定轴旋转的，绕x轴，再绕y轴，再绕z轴，所绕的轴是一直不变的。

因为setRPY接收的参数的顺序为roll,pitch,yall；

pitch()：俯仰，将物体绕X轴旋转（localRotationX）；

yaw()：航向，将物体绕Y轴旋转（localRotationY）；

roll()：横滚，将物体绕Z轴旋转（localRotationZ）；

2.setEuler；（姿态角（Euler角）：yaw pitch roll）
这种方式是绕着动轴转动，先绕Y轴，在绕变换后的X轴，再绕变换后的Z轴旋转。
从数学形式上说，这是绕定轴YXZ矩阵依次右乘，即：R = R(y)R(x)R(z) 的顺序。

3.对于这个x y z yaw pitch roll是怎么解析的;
基本可以理解为：
将子坐标系移动到parent坐标系的(x,y,z)位置下，然后子坐标系绕自己的动轴旋转，按照ZYX的顺序。
绕动轴旋转，在参数设置上很不直观啊！

4.1设置ros中xyz和pry;

transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(), "turtle1", "carrot1")

这三句话分别做了以下工作：

1、设置carrot1在turtle1坐标系下的坐标原点 ;
2、设置carrot1相对于turtle1的旋转角度，这里用四元数表示 ;
3、发送变换信息 ;

上面四元数表示旋转角度的方式不太直观，我们写代码的时候不想将旋转变换换算成四元数的时候可以采用如下方法写这个变换：

static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

这样通过这三句话就可以直接用RPY（分别对应绕XYZ轴旋转角度）来设置旋转变换了

tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);

这里的msg->x, msg->y,msg->theta是解引用msg并获取该元素名为x（或者y，theta）的成员，相当于(*msg).x，(*msg).y，(*msg).theta ;

4.2 static_transform_publisher

static_transform_publisher工具的功能是发布两个参考系之间的静态坐标变换，两个参考系一般不发生相对位置变化。

命令的格式如下：

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms

以上两种命令格式，需要设置坐标的偏移和旋转参数，偏移参数都使用相对于xyz三轴的坐标位移，而旋转参数第一种命令格式使用以弧度为单位的 yaw/pitch/roll三个角度（yaw是围绕x轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕z轴旋转的翻滚角），而第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位，一般100ms比较合适。