3ros2_python/vis_ros2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 总结\n",
    "文档：https://www.yuque.com/huangzhongqing/ld627o/gulyke"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 可视化点云\n",
    "* 运行下面代码，然后在终端打开`rviz2`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'rclpy'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[1;32m/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb Cell 3\u001b[0m in \u001b[0;36m<cell line: 17>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=0'>1</a>\u001b[0m \u001b[39m# Copyright 2020 Evan Flynn\u001b[39;00m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=1'>2</a>\u001b[0m \u001b[39m#\u001b[39;00m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=2'>3</a>\u001b[0m \u001b[39m# Licensed under the Apache License, Version 2.0 (the \"License\");\u001b[39;00m\n\u001b[0;32m   (...)\u001b[0m\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=11'>12</a>\u001b[0m \u001b[39m# See the License for the specific language governing permissions and\u001b[39;00m\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=12'>13</a>\u001b[0m \u001b[39m# limitations under the License.\u001b[39;00m\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=14'>15</a>\u001b[0m \u001b[39mimport\u001b[39;00m \u001b[39mnumpy\u001b[39;00m \u001b[39mas\u001b[39;00m \u001b[39mnp\u001b[39;00m\n\u001b[0;32m---> <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=16'>17</a>\u001b[0m \u001b[39mimport\u001b[39;00m \u001b[39mrclpy\u001b[39;00m\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=17'>18</a>\u001b[0m \u001b[39mfrom\u001b[39;00m \u001b[39mrclpy\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mnode\u001b[39;00m \u001b[39mimport\u001b[39;00m Node\n\u001b[1;32m     <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000002?line=18'>19</a>\u001b[0m \u001b[39mfrom\u001b[39;00m \u001b[39msensor_msgs\u001b[39;00m\u001b[39m.\u001b[39;00m\u001b[39mmsg\u001b[39;00m \u001b[39mimport\u001b[39;00m PointCloud2\n",
      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'rclpy'"
     ]
    }
   ],
   "source": [
    "# Copyright 2020 Evan Flynn\n",
    "#\n",
    "# Licensed under the Apache License, Version 2.0 (the \"License\");\n",
    "# you may not use this file except in compliance with the License.\n",
    "# You may obtain a copy of the License at\n",
    "#\n",
    "#     http://www.apache.org/licenses/LICENSE-2.0\n",
    "#\n",
    "# Unless required by applicable law or agreed to in writing, software\n",
    "# distributed under the License is distributed on an \"AS IS\" BASIS,\n",
    "# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n",
    "# See the License for the specific language governing permissions and\n",
    "# limitations under the License.\n",
    "\n",
    "import numpy as np\n",
    "\n",
    "import rclpy\n",
    "from rclpy.node import Node\n",
    "from sensor_msgs.msg import PointCloud2\n",
    "from sensor_msgs.msg import PointField\n",
    "from sensor_msgs_py import point_cloud2\n",
    "from std_msgs.msg import Header\n",
    "\n",
    "\n",
    "class PointCloudPublisher(Node):\n",
    "\n",
    "    rate = 30\n",
    "    moving = True\n",
    "    width = 100\n",
    "    height = 100\n",
    "\n",
    "    header = Header()\n",
    "    header.frame_id = 'map'\n",
    "\n",
    "    dtype = PointField.FLOAT32\n",
    "    point_step = 16\n",
    "    fields = [PointField(name='x', offset=0, datatype=dtype, count=1),\n",
    "              PointField(name='y', offset=4, datatype=dtype, count=1),\n",
    "              PointField(name='z', offset=8, datatype=dtype, count=1),\n",
    "              PointField(name='intensity', offset=12, datatype=dtype, count=1)]\n",
    "\n",
    "    def __init__(self):\n",
    "        super().__init__('pc_publisher')\n",
    "        self.publisher_ = self.create_publisher(PointCloud2, '/raw_cloud', 10)\n",
    "        timer_period = 1 / self.rate\n",
    "        self.timer = self.create_timer(timer_period, self.timer_callback)\n",
    "        self.counter = 0\n",
    "\n",
    "    def timer_callback(self):\n",
    "        self.header.stamp = self.get_clock().now().to_msg()\n",
    "        x, y = np.meshgrid(np.linspace(-2, 2, self.width), np.linspace(-2, 2, self.height))\n",
    "        z = 0.5 * np.sin(2*x-self.counter/10.0) * np.sin(2*y)\n",
    "        points = np.array([x, y, z, z]).reshape(4, -1).T\n",
    "        pc2_msg = point_cloud2.create_cloud(self.header, self.fields, points)\n",
    "        self.publisher_.publish(pc2_msg)\n",
    "\n",
    "        if self.moving:\n",
    "            self.counter += 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'rclpy' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb Cell 4\u001b[0m in \u001b[0;36m<cell line: 9>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=5'>6</a>\u001b[0m     rclpy\u001b[39m.\u001b[39mshutdown()\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=8'>9</a>\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39m__name__\u001b[39m \u001b[39m==\u001b[39m \u001b[39m'\u001b[39m\u001b[39m__main__\u001b[39m\u001b[39m'\u001b[39m:\n\u001b[0;32m---> <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=9'>10</a>\u001b[0m     main()\n",
      "\u001b[1;32m/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb Cell 4\u001b[0m in \u001b[0;36mmain\u001b[0;34m(args)\u001b[0m\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=0'>1</a>\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mmain\u001b[39m(args\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m):\n\u001b[0;32m----> <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=1'>2</a>\u001b[0m     rclpy\u001b[39m.\u001b[39minit(args\u001b[39m=\u001b[39margs)\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=2'>3</a>\u001b[0m     pc_publisher \u001b[39m=\u001b[39m PointCloudPublisher()\n\u001b[1;32m      <a href='vscode-notebook-cell:/home/chongqinghuang/code/Point-Clouds-Visualization/3ros2_python/vis_ros2.ipynb#ch0000003?line=3'>4</a>\u001b[0m     rclpy\u001b[39m.\u001b[39mspin(pc_publisher)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'rclpy' is not defined"
     ]
    }
   ],
   "source": [
    "def main(args=None):\n",
    "    rclpy.init(args=args)\n",
    "    pc_publisher = PointCloudPublisher()\n",
    "    rclpy.spin(pc_publisher)\n",
    "    pc_publisher.destroy_node()\n",
    "    rclpy.shutdown()\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 可视化点云（with color）\n",
    "* 点云分割赋予不同label的点颜色"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 可视化bbox\n",
    "* 点云目标检测"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.13 ('waymo_38')",
   "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.13"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "44c80fdca727e273b3b1fdcfb86e5664b0451e88fa74e3d1ad2cb050c7994d10"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}