{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "1ede788b",
   "metadata": {},
   "source": [
    "1.导入rclpy和Node"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "2b0240ea",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "import rclpy\n",
    "from rclpy.node import Node"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "586e8ab6",
   "metadata": {},
   "source": [
    "2.导入TF帧：TF帧对应的消息接口为geometry_msgs.msg下的TransformStamped"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "69cefff7",
   "metadata": {},
   "outputs": [],
   "source": [
    "from geometry_msgs.msg import TransformStamped"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de0410de",
   "metadata": {},
   "source": [
    "3.从tf2_ros包中导入坐标变换广播器"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "b3e88382",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tf2_ros import TransformBroadcaster"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72808853",
   "metadata": {},
   "source": [
    "4.初始化节点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "c1421620",
   "metadata": {},
   "outputs": [],
   "source": [
    "rclpy.init()\n",
    "node = Node(\"transform_node2\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "400707c5",
   "metadata": {},
   "source": [
    "5.构造静态广播发布器"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "70636885",
   "metadata": {},
   "outputs": [],
   "source": [
    "tf_pub = TransformBroadcaster(node)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b0b9127",
   "metadata": {},
   "source": [
    "6.构造TF帧，，其要发布出去的消息"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "156e8419",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = TransformStamped()\n",
    "t.header.stamp = node.get_clock().now().to_msg()\n",
    "# parent-name:相机坐标系C\n",
    "t.header.frame_id = 'C'\n",
    "# chrild-name：工件坐标系P\n",
    "t.child_frame_id = 'P'\n",
    "# 平移关系，单位m\n",
    "t.transform.translation.x = 2.0\n",
    "t.transform.translation.y = 1.0\n",
    "t.transform.translation.z = 2.0\n",
    "# 旋转关系，四元数形式，我们需要将欧拉角的形式转换成四元数\n",
    "# 可以使用在线坐标转换工具：https://quaternions.online/\n",
    "t.transform.rotation.x = 1.0\n",
    "t.transform.rotation.y = 0.0\n",
    "t.transform.rotation.z = 0.0\n",
    "t.transform.rotation.w = 0.0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "009c3752",
   "metadata": {},
   "source": [
    "7.以10Hz发布坐标关系"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "117b1595",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def send_transform():\n",
    "    t.header.stamp = node.get_clock().now().to_msg()\n",
    "    tf_pub.sendTransform(t)\n",
    "node.create_timer(0.1,send_transform)\n",
    "rclpy.spin(node)"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "原始单元格格式",
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}