ROS tf使用

1,静态tf发布

<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z yaw pitch raw link1 link2 100" />

<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z qx qy qz qw link1 link2 100" />

2,动态tf发布

#include <tf/transform_broadcaster.h>

  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(x, y, z) );
  tf::Quaternion q;
  q.setRPY(0, 0,yaw);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "my_frame"));

broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));

3, 四元数和欧拉角之间的变换

3.1 四元数->欧拉角

C++:
geometry_msgs::Quaternion geom_quat
  tf::Quaternion tf_quat_;
  tf::quaternionMsgToTF(geom_quat, tf_quat_); 
  double roll, pitch, yaw;//
  tf::Matrix3x3(tf_quat_).getRPY(roll, pitch, yaw);

python:
import roslib
roslib.load_manifest('learning_tf')
import rospy

quaternion = (
    pose.orientation.x,
    pose.orientation.y,
    pose.orientation.z,
    pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]

3.2 欧拉角->四元数

C++:
  tf::Quaternion q;
  q.setRPY(0, 0,yaw);

python:
 q = quaternion_from_euler(1.5707, 0, -1.5707)
 x = quaternion[0]
 y = quaternion[1]
 z = quaternion[2]
 w = quaternion[3]