{
 "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\n",
    "其结构可以使用下面指令查看：\n",
    "\n",
    "```\n",
    "!ros2 interface show geometry_msgs/msg/TransformStamped\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "6a3feb9f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "# This expresses a transform from coordinate frame header.frame_id\r\n",
      "# to the coordinate frame child_frame_id at the time of header.stamp\r\n",
      "#\r\n",
      "# This message is mostly used by the\r\n",
      "# <a href=\"https://index.ros.org/p/tf2/\">tf2</a> package.\r\n",
      "# See its documentation for more information.\r\n",
      "#\r\n",
      "# The child_frame_id is necessary in addition to the frame_id\r\n",
      "# in the Header to communicate the full reference for the transform\r\n",
      "# in a self contained message.\r\n",
      "\r\n",
      "# The frame id in the header is used as the reference frame of this transform.\r\n",
      "std_msgs/Header header\r\n",
      "\r\n",
      "# The frame id of the child frame to which this transform points.\r\n",
      "string child_frame_id\r\n",
      "\r\n",
      "# Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.\r\n",
      "Transform transform\r\n"
     ]
    }
   ],
   "source": [
    "!ros2 interface show 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": 13,
   "id": "117b1595",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n",
      "transforms\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_7847/1847487335.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      3\u001b[0m     \u001b[0mtf_pub\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msendTransform\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[0mnode\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcreate_timer\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m0.1\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0msend_transform\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 5\u001b[0;31m \u001b[0mrclpy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mspin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnode\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;32m/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py\u001b[0m in \u001b[0;36mspin\u001b[0;34m(node, executor)\u001b[0m\n\u001b[1;32m    189\u001b[0m         \u001b[0mexecutor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0madd_node\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnode\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    190\u001b[0m         \u001b[0;32mwhile\u001b[0m \u001b[0mexecutor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcontext\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mok\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 191\u001b[0;31m             \u001b[0mexecutor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mspin_once\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    192\u001b[0m     \u001b[0;32mfinally\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    193\u001b[0m         \u001b[0mexecutor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mremove_node\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnode\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py\u001b[0m in \u001b[0;36mspin_once\u001b[0;34m(self, timeout_sec)\u001b[0m\n\u001b[1;32m    704\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0mspin_once\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtimeout_sec\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mfloat\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m->\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    705\u001b[0m         \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 706\u001b[0;31m             \u001b[0mhandler\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mentity\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mnode\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mwait_for_ready_callbacks\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtimeout_sec\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mtimeout_sec\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    707\u001b[0m         \u001b[0;32mexcept\u001b[0m \u001b[0mShutdownException\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    708\u001b[0m             \u001b[0;32mpass\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py\u001b[0m in \u001b[0;36mwait_for_ready_callbacks\u001b[0;34m(self, *args, **kwargs)\u001b[0m\n\u001b[1;32m    690\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    691\u001b[0m             \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 692\u001b[0;31m                 \u001b[0;32mreturn\u001b[0m \u001b[0mnext\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_cb_iter\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    693\u001b[0m             \u001b[0;32mexcept\u001b[0m \u001b[0mStopIteration\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    694\u001b[0m                 \u001b[0;31m# Generator ran out of work\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py\u001b[0m in \u001b[0;36m_wait_for_ready_callbacks\u001b[0;34m(self, timeout_sec, nodes, condition)\u001b[0m\n\u001b[1;32m    587\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    588\u001b[0m                 \u001b[0;31m# Wait for something to become ready\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 589\u001b[0;31m                 \u001b[0m_rclpy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mrclpy_wait\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mwait_set\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtimeout_nsec\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    590\u001b[0m                 \u001b[0;32mif\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_is_shutdown\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    591\u001b[0m                     \u001b[0;32mraise\u001b[0m \u001b[0mShutdownException\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "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)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "21c24746",
   "metadata": {},
   "source": [
    "使用命令行监听坐标关系\n",
    "```\n",
    "ros2 run tf2_ros tf2_echo C P\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1fc3d36e",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "!ros2 run tf2_ros tf2_echo C P"
   ]
  }
 ],
 "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
}